├── .github ├── ISSUE_TEMPLATE │ ├── bug_report.md │ ├── custom.md │ └── feature_request.md └── workflows │ └── workflow.yml ├── .gitignore ├── LICENSE.txt ├── README.md ├── bt_navigators ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── behavior_trees │ ├── follow_point.xml │ ├── navigate_through_poses_w_replanning_and_recovery.xml │ ├── navigate_to_pose_w_replanning_and_recovery.xml │ ├── navigate_w_replanning_distance.xml │ ├── navigate_w_replanning_speed.xml │ └── navigate_w_replanning_time.xml ├── include │ └── bt_navigators │ │ ├── bt_navigator_ab.hpp │ │ ├── bt_navigator_docking.hpp │ │ ├── bt_navigator_tracking.hpp │ │ ├── navigator.hpp │ │ └── navigators │ │ ├── auto_docking.hpp │ │ ├── navigate_pose.hpp │ │ └── target_tracking.hpp ├── package.xml └── src │ ├── bt_navigator_ab.cpp │ ├── bt_navigator_docking.cpp │ ├── bt_navigator_tracking.cpp │ └── navigators │ ├── auto_docking.cpp │ ├── navigate_pose.cpp │ └── target_tracking.cpp ├── mcr_bringup ├── CMakeLists.txt ├── README.md ├── behavior_trees │ ├── auto_docking.xml │ ├── automatic_recharge.xml │ ├── automatic_recharge_old.xml │ ├── navigate_to_pose_w_replanning_and_recovery.xml │ └── target_tracking.xml ├── launch │ ├── bringup_dock_launch.py │ ├── bringup_follow_launch.py │ ├── bringup_follow_only_launch.py │ └── bringup_navigate_to_pose_launch.py ├── package.xml └── params │ ├── auto_charging.yaml │ ├── auto_docking.yaml │ ├── follow_params.yaml │ ├── follow_params.yaml.bak │ ├── nav2_params.yaml │ ├── navigate_to_pose_params.yaml │ └── recoveries_params.yaml ├── mcr_controller ├── .gitignore ├── CMakeLists.txt ├── README.md ├── include │ └── mcr_controller │ │ └── controller_server.hpp ├── package.xml └── src │ ├── controller_server.cpp │ └── main.cpp ├── mcr_global_planner ├── .gitignore ├── CMakeLists.txt ├── include │ └── mcr_global_planner │ │ └── mcr_spline_planner.hpp ├── launch │ └── mcr_planner.launch.py ├── mcr_global_planner_plugin.xml ├── package.xml ├── params │ └── planner_params.yaml └── src │ └── mcr_spline_planner.cpp ├── mcr_msgs ├── CMakeLists.txt ├── README.md ├── action │ ├── AutomaticRecharge.action │ ├── ComputePathSplinePoses.action │ └── TargetTracking.action ├── package.xml └── srv │ └── SwitchMode.srv ├── mcr_planner ├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── mcr_planner │ │ └── planner_server.hpp ├── package.xml └── src │ ├── main.cpp │ └── planner_server.cpp ├── mcr_tracking_components ├── .gitignore ├── CMakeLists.txt ├── README.md ├── behavior_tree_nodes.xml ├── controller_plugins.xml ├── costmap_plugins.xml ├── dwb_critics.xml ├── include │ └── mcr_tracking_components │ │ ├── behavior_tree_nodes │ │ ├── change_gait_node.hpp │ │ ├── charger_updater_node.hpp │ │ ├── compute_path_spline_poses_action.hpp │ │ ├── compute_path_to_pose_action.hpp │ │ ├── exception_verify_node.hpp │ │ ├── follow_path_action.hpp │ │ ├── is_battery_in_charge_condition.hpp │ │ ├── orientation_derivers.hpp │ │ ├── seat_adjust_client_node.hpp │ │ ├── spin_and_search_action.hpp │ │ ├── target_updater_node.hpp │ │ └── tracking_mode_decider_node.hpp │ │ ├── controller_plugins │ │ ├── fake_progress_checker.hpp │ │ ├── unreachable_goal_checker.hpp │ │ ├── x_goal_checker.hpp │ │ └── xy_goal_checker.hpp │ │ ├── costmap_layers │ │ └── obstacle_layer.hpp │ │ ├── dwb_critics │ │ ├── keep_person_insight.hpp │ │ └── keep_target_insight.hpp │ │ └── recoveries │ │ └── spin_and_search.hpp ├── orientation_derivers.xml ├── package.xml ├── recoveries.xml └── src │ ├── behavior_tree_nodes │ ├── change_gait_node.cpp │ ├── charger_updater_node.cpp │ ├── compute_path_spline_poses_action.cpp │ ├── compute_path_to_pose_action.cpp │ ├── exception_verify_node.cpp │ ├── follow_path_action.cpp │ ├── is_battery_in_charge_condition.cpp │ ├── orientation_derivers.cpp │ ├── seat_adjust_client_node.cpp │ ├── spin_and_search_action.cpp │ ├── target_updater_node.cpp │ └── tracking_mode_decider_node.cpp │ ├── controller_plugins │ ├── fake_progress_checker.cpp │ ├── unreachable_goal_checker.cpp │ ├── x_goal_checker.cpp │ └── xy_goal_checker.cpp │ ├── costmap_layers │ └── obstacle_layer.cpp │ ├── dwb_critics │ ├── keep_person_insight.cpp │ └── keep_target_insight.cpp │ └── recoveries │ └── spin_and_search.cpp ├── mcr_uwb ├── CMakeLists.txt ├── README.md ├── include │ └── mcr_uwb │ │ └── mcr_uwb.hpp ├── launch │ └── uwb_launch.py ├── package.xml └── src │ ├── main.cpp │ └── mcr_uwb.cpp └── mcr_voice ├── CMakeLists.txt ├── README.md ├── include └── mcr_voice │ └── mcr_voice.hpp ├── launch └── voice_launch.py ├── package.xml └── src ├── main.cpp └── mcr_voice.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 | curl -s https://packagecloud.io/install/repositories/dirk-thomas/vcstool/script.deb.sh | sudo bash 21 | sudo apt-get update 22 | sudo apt-get install python3-vcstool 23 | - name: Download repository 24 | run: | 25 | git clone https://github.com/MiRoboticsLab/cyberdog_ws.git 26 | vcs import . < cyberdog_ws/.github/ci.repos 27 | - name: Build and code test 28 | run: | 29 | docker pull ghcr.io/miroboticslab/cyberdog:v1 30 | docker run -i -v $GITHUB_WORKSPACE:/home/ros2/src ghcr.io/miroboticslab/cyberdog:v1 bash -c \ 31 | "cd /home/ros2 && source /opt/ros2/galactic/setup.bash \ 32 | && colcon build --merge-install --packages-up-to mcr_bringup mcr_msgs mcr_uwb mcr_voice bt_navigators \ 33 | && colcon test --merge-instal --event-handlers console_cohesion+ --return-code-on-test-failure --packages-select mcr_bringup mcr_msgs mcr_uwb mcr_voice bt_navigators " 34 | # colcon build test 35 | -------------------------------------------------------------------------------- /.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 tracking base 2 | 3 | 主要存放了基于navigation2实现的docking， navigation， tracking功能相关的参数，附加模块等。 4 | 5 | ## mcr_bringup 6 | 7 | 存放docking，navigation， tracking三个功能所用到的参数，行为树，launch等，详细内容可参考：[mcr_bringup](mcr_bringup/README.md) 8 | 9 | ## mcr_voice 10 | 11 | 订阅tracking功能执行任务时的feedback信息，并根据feedback进行语音播报，详细内容可参考：[mcr_voice](mcr_voice/README.md) 12 | 13 | ## mcr_uwb 14 | 15 | 订阅uwb(Ultra Wide Band)驱动数据，并转换为geometry_msgs::PoseStamped数据，详细内容可参考：[mcr_uwb](mcr_uwb/README.md) 16 | 17 | ## mcr_msgs 18 | 19 | 定义了跟随功能相关的接口，详细可参考：[mcr_msgs](mcr_msgs/README.md) 20 | 21 | ## bt_navigators 22 | 23 | 修改自nav2_bt_navigators，做了功能拆分，目的是为了缓解功能启动时加载时间过长的问题，详细可参考：[bt_navigators](bt_navigators/README.md) 24 | -------------------------------------------------------------------------------- /bt_navigators/CHANGELOG.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_tracking_base/0bb7dfa9fc22e1c877c3ca753c0a663b3531c814/bt_navigators/CHANGELOG.rst -------------------------------------------------------------------------------- /bt_navigators/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(bt_navigators) 3 | add_compile_options(-g -O0) 4 | find_package(ament_cmake REQUIRED) 5 | find_package(nav2_common REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(rclcpp_action REQUIRED) 8 | find_package(rclcpp_lifecycle REQUIRED) 9 | find_package(std_msgs REQUIRED) 10 | find_package(geometry_msgs REQUIRED) 11 | find_package(nav2_behavior_tree REQUIRED) 12 | find_package(nav_msgs REQUIRED) 13 | find_package(nav2_msgs REQUIRED) 14 | find_package(mcr_msgs REQUIRED) 15 | find_package(behaviortree_cpp_v3 REQUIRED) 16 | find_package(std_srvs REQUIRED) 17 | find_package(nav2_util REQUIRED) 18 | find_package(nav2_core REQUIRED) 19 | find_package(tf2_ros REQUIRED) 20 | find_package(protocol REQUIRED) 21 | find_package(cyberdog_debug REQUIRED) 22 | find_package(cyberdog_common REQUIRED) 23 | nav2_package() 24 | 25 | include_directories( 26 | include 27 | ) 28 | 29 | set(library_name bt_navigators_core) 30 | 31 | set(dependencies 32 | rclcpp 33 | rclcpp_action 34 | rclcpp_lifecycle 35 | std_msgs 36 | geometry_msgs 37 | nav2_behavior_tree 38 | nav_msgs 39 | nav2_msgs 40 | mcr_msgs 41 | behaviortree_cpp_v3 42 | std_srvs 43 | nav2_util 44 | nav2_core 45 | tf2_ros 46 | protocol 47 | cyberdog_debug 48 | cyberdog_common 49 | ) 50 | 51 | add_library(${library_name} SHARED 52 | src/navigators/target_tracking.cpp 53 | src/navigators/auto_docking.cpp 54 | src/navigators/navigate_pose.cpp 55 | ) 56 | 57 | 58 | # bt_navigator_tracking 59 | add_executable(bt_navigator_tracking src/bt_navigator_tracking.cpp) 60 | target_link_libraries(bt_navigator_tracking ${library_name}) 61 | ament_target_dependencies(bt_navigator_tracking ${dependencies}) 62 | 63 | # bt_navigator_docking 64 | add_executable(bt_navigator_docking src/bt_navigator_docking.cpp) 65 | target_link_libraries(bt_navigator_docking ${library_name}) 66 | ament_target_dependencies(bt_navigator_docking ${dependencies}) 67 | 68 | # bt_navigator_pose 69 | add_executable(bt_navigator_pose src/bt_navigator_ab.cpp) 70 | target_link_libraries(bt_navigator_pose ${library_name}) 71 | ament_target_dependencies(bt_navigator_pose ${dependencies}) 72 | 73 | ament_target_dependencies(${library_name} 74 | ${dependencies} 75 | ) 76 | 77 | install(TARGETS ${library_name} 78 | ARCHIVE DESTINATION lib 79 | LIBRARY DESTINATION lib 80 | RUNTIME DESTINATION bin 81 | ) 82 | 83 | install(TARGETS bt_navigator_tracking 84 | RUNTIME DESTINATION lib/${PROJECT_NAME} 85 | ) 86 | 87 | install(TARGETS bt_navigator_pose 88 | RUNTIME DESTINATION lib/${PROJECT_NAME} 89 | ) 90 | 91 | install(TARGETS bt_navigator_docking 92 | RUNTIME DESTINATION lib/${PROJECT_NAME} 93 | ) 94 | 95 | install(DIRECTORY include/ 96 | DESTINATION include/ 97 | ) 98 | 99 | install(DIRECTORY behavior_trees DESTINATION share/${PROJECT_NAME}) 100 | if(BUILD_TESTING) 101 | find_package(ament_lint_auto REQUIRED) 102 | ament_lint_auto_find_test_dependencies() 103 | endif() 104 | 105 | ament_export_include_directories(include) 106 | ament_export_libraries(${library_name}) 107 | ament_export_dependencies(${dependencies}) 108 | ament_package() 109 | -------------------------------------------------------------------------------- /bt_navigators/README.md: -------------------------------------------------------------------------------- 1 | # MCR Navigator 2 | 3 | 功能包继承于[nav2_bt_navigator](https://github.com/ros-planning/navigation2/tree/galactic/nav2_bt_navigator),由于跟随，导航，回充功能都基于nav2_bt_navigator实现，导致单体功能启动时，配置过程较久，通过分离功能达到快速启动的效果。 4 | -------------------------------------------------------------------------------- /bt_navigators/behavior_trees/follow_point.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /bt_navigators/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /bt_navigators/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /bt_navigators/behavior_trees/navigate_w_replanning_distance.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /bt_navigators/behavior_trees/navigate_w_replanning_speed.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /bt_navigators/behavior_trees/navigate_w_replanning_time.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /bt_navigators/include/bt_navigators/bt_navigator_ab.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel 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 | #ifndef BT_NAVIGATORS__BT_NAVIGATOR_AB_HPP_ 16 | #define BT_NAVIGATORS__BT_NAVIGATOR_AB_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "nav2_util/lifecycle_node.hpp" 23 | #include "rclcpp_action/rclcpp_action.hpp" 24 | #include "tf2_ros/buffer.h" 25 | #include "tf2_ros/transform_listener.h" 26 | #include "tf2_ros/create_timer_ros.h" 27 | #include "bt_navigators/navigators/navigate_pose.hpp" 28 | 29 | 30 | namespace bt_navigators 31 | { 32 | 33 | /** 34 | * @class nav2_bt_navigator::BtNavigator 35 | * @brief An action server that uses behavior tree for navigating a robot to its 36 | * goal position. 37 | */ 38 | class BtPoseNavigator : public nav2_util::LifecycleNode 39 | { 40 | public: 41 | /** 42 | * @brief A constructor for nav2_bt_navigator::BtNavigator class 43 | */ 44 | BtPoseNavigator(); 45 | /** 46 | * @brief A destructor for nav2_bt_navigator::BtNavigator class 47 | */ 48 | ~BtPoseNavigator(); 49 | 50 | protected: 51 | /** 52 | * @brief Configures member variables 53 | * 54 | * Initializes action server for "NavigationToPose"; subscription to 55 | * "goal_sub"; and builds behavior tree from xml file. 56 | * @param state Reference to LifeCycle node state 57 | * @return SUCCESS or FAILURE 58 | */ 59 | nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; 60 | /** 61 | * @brief Activates action server 62 | * @param state Reference to LifeCycle node state 63 | * @return SUCCESS or FAILURE 64 | */ 65 | nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; 66 | /** 67 | * @brief Deactivates action server 68 | * @param state Reference to LifeCycle node state 69 | * @return SUCCESS or FAILURE 70 | */ 71 | nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; 72 | /** 73 | * @brief Resets member variables 74 | * @param state Reference to LifeCycle node state 75 | * @return SUCCESS or FAILURE 76 | */ 77 | nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; 78 | /** 79 | * @brief Called when in shutdown state 80 | * @param state Reference to LifeCycle node state 81 | * @return SUCCESS or FAILURE 82 | */ 83 | nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; 84 | 85 | // To handle all the BT related execution 86 | std::unique_ptr> pose_navigator_; 87 | bt_navigators::NavigatorMuxer plugin_muxer_; 88 | 89 | // Odometry smoother object 90 | std::unique_ptr odom_smoother_; 91 | 92 | // Metrics for feedback 93 | std::string robot_frame_; 94 | std::string global_frame_; 95 | double transform_tolerance_; 96 | std::string odom_topic_; 97 | 98 | // Spinning transform that can be used by the BT nodes 99 | std::shared_ptr tf_; 100 | std::shared_ptr tf_listener_; 101 | }; 102 | 103 | } // namespace bt_navigators 104 | 105 | #endif // BT_NAVIGATORS__BT_NAVIGATOR_AB_HPP_ 106 | -------------------------------------------------------------------------------- /bt_navigators/include/bt_navigators/bt_navigator_docking.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel 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 | #ifndef BT_NAVIGATORS__BT_NAVIGATOR_DOCKING_HPP_ 16 | #define BT_NAVIGATORS__BT_NAVIGATOR_DOCKING_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "nav2_util/lifecycle_node.hpp" 23 | #include "rclcpp_action/rclcpp_action.hpp" 24 | #include "tf2_ros/buffer.h" 25 | #include "tf2_ros/transform_listener.h" 26 | #include "tf2_ros/create_timer_ros.h" 27 | #include "bt_navigators/navigators/auto_docking.hpp" 28 | 29 | namespace bt_navigators 30 | { 31 | 32 | /** 33 | * @class bt_navigators::BtNavigatorDock 34 | * @brief An action server that uses behavior tree for navigating a robot to its 35 | * goal position. 36 | */ 37 | class BtNavigatorDock : public nav2_util::LifecycleNode 38 | { 39 | public: 40 | /** 41 | * @brief A constructor for bt_navigators::BtNavigatorDock class 42 | */ 43 | BtNavigatorDock(); 44 | /** 45 | * @brief A destructor for bt_navigators::BtNavigatorDock class 46 | */ 47 | ~BtNavigatorDock(); 48 | 49 | protected: 50 | /** 51 | * @brief Configures member variables 52 | * 53 | * Initializes action server for "NavigationToPose"; subscription to 54 | * "goal_sub"; and builds behavior tree from xml file. 55 | * @param state Reference to LifeCycle node state 56 | * @return SUCCESS or FAILURE 57 | */ 58 | nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; 59 | /** 60 | * @brief Activates action server 61 | * @param state Reference to LifeCycle node state 62 | * @return SUCCESS or FAILURE 63 | */ 64 | nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; 65 | /** 66 | * @brief Deactivates action server 67 | * @param state Reference to LifeCycle node state 68 | * @return SUCCESS or FAILURE 69 | */ 70 | nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; 71 | /** 72 | * @brief Resets member variables 73 | * @param state Reference to LifeCycle node state 74 | * @return SUCCESS or FAILURE 75 | */ 76 | nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; 77 | /** 78 | * @brief Called when in shutdown state 79 | * @param state Reference to LifeCycle node state 80 | * @return SUCCESS or FAILURE 81 | */ 82 | nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; 83 | 84 | // To handle all the BT related execution 85 | std::unique_ptr> 86 | automatic_recharge_navigator_; 87 | bt_navigators::NavigatorMuxer plugin_muxer_; 88 | 89 | // Odometry smoother object 90 | std::unique_ptr odom_smoother_; 91 | 92 | // Metrics for feedback 93 | std::string robot_frame_; 94 | std::string global_frame_; 95 | double transform_tolerance_; 96 | std::string odom_topic_; 97 | 98 | // Spinning transform that can be used by the BT nodes 99 | std::shared_ptr tf_; 100 | std::shared_ptr tf_listener_; 101 | }; 102 | 103 | } // namespace bt_navigators 104 | 105 | #endif // BT_NAVIGATORS__BT_NAVIGATOR_DOCKING_HPP_ 106 | -------------------------------------------------------------------------------- /bt_navigators/include/bt_navigators/bt_navigator_tracking.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel 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 | #ifndef BT_NAVIGATORS__BT_NAVIGATOR_TRACKING_HPP_ 16 | #define BT_NAVIGATORS__BT_NAVIGATOR_TRACKING_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "nav2_util/lifecycle_node.hpp" 23 | #include "rclcpp_action/rclcpp_action.hpp" 24 | #include "tf2_ros/buffer.h" 25 | #include "tf2_ros/transform_listener.h" 26 | #include "tf2_ros/create_timer_ros.h" 27 | #include "bt_navigators/navigators/target_tracking.hpp" 28 | 29 | namespace bt_navigators 30 | { 31 | 32 | /** 33 | * @class bt_navigators::BtNavigator 34 | * @brief An action server that uses behavior tree for navigating a robot to its 35 | * goal position. 36 | */ 37 | class BtNavigatorTracking : public nav2_util::LifecycleNode 38 | { 39 | public: 40 | /** 41 | * @brief A constructor for bt_navigators::BtNavigator class 42 | */ 43 | BtNavigatorTracking(); 44 | /** 45 | * @brief A destructor for bt_navigators::BtNavigator class 46 | */ 47 | ~BtNavigatorTracking(); 48 | 49 | protected: 50 | /** 51 | * @brief Configures member variables 52 | * 53 | * Initializes action server for "NavigationToPose"; subscription to 54 | * "goal_sub"; and builds behavior tree from xml file. 55 | * @param state Reference to LifeCycle node state 56 | * @return SUCCESS or FAILURE 57 | */ 58 | nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; 59 | /** 60 | * @brief Activates action server 61 | * @param state Reference to LifeCycle node state 62 | * @return SUCCESS or FAILURE 63 | */ 64 | nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; 65 | /** 66 | * @brief Deactivates action server 67 | * @param state Reference to LifeCycle node state 68 | * @return SUCCESS or FAILURE 69 | */ 70 | nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; 71 | /** 72 | * @brief Resets member variables 73 | * @param state Reference to LifeCycle node state 74 | * @return SUCCESS or FAILURE 75 | */ 76 | nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; 77 | /** 78 | * @brief Called when in shutdown state 79 | * @param state Reference to LifeCycle node state 80 | * @return SUCCESS or FAILURE 81 | */ 82 | nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; 83 | 84 | // To handle all the BT related execution 85 | std::unique_ptr> 86 | target_tracking_navigator_; 87 | bt_navigators::NavigatorMuxer plugin_muxer_; 88 | 89 | // Odometry smoother object 90 | std::unique_ptr odom_smoother_; 91 | 92 | // Metrics for feedback 93 | std::string robot_frame_; 94 | std::string global_frame_; 95 | double transform_tolerance_; 96 | std::string odom_topic_; 97 | 98 | // Spinning transform that can be used by the BT nodes 99 | std::shared_ptr tf_; 100 | std::shared_ptr tf_listener_; 101 | }; 102 | 103 | } // namespace bt_navigators 104 | 105 | #endif // BT_NAVIGATORS__BT_NAVIGATOR_TRACKING_HPP_ 106 | -------------------------------------------------------------------------------- /bt_navigators/include/bt_navigators/navigators/auto_docking.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Samsung Research 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef BT_NAVIGATORS__NAVIGATORS__AUTO_DOCKING_HPP_ 16 | #define BT_NAVIGATORS__NAVIGATORS__AUTO_DOCKING_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "geometry_msgs/msg/pose_stamped.hpp" 23 | #include "mcr_msgs/action/automatic_recharge.hpp" 24 | #include "bt_navigators/navigator.hpp" 25 | #include "nav2_util/geometry_utils.hpp" 26 | #include "nav2_util/odometry_utils.hpp" 27 | #include "nav2_util/robot_utils.hpp" 28 | #include "nav_msgs/msg/path.hpp" 29 | #include "rclcpp/rclcpp.hpp" 30 | #include "rclcpp_action/rclcpp_action.hpp" 31 | 32 | namespace bt_navigators 33 | { 34 | 35 | /** 36 | * @class AutoDockingNavigator 37 | * @brief A navigator for tracking target 38 | */ 39 | class AutoDockingNavigator 40 | : public bt_navigators::Navigator 41 | { 42 | public: 43 | using ActionT = mcr_msgs::action::AutomaticRecharge; 44 | 45 | /** 46 | * @brief A constructor for AutoDockingNavigator 47 | */ 48 | AutoDockingNavigator() 49 | : Navigator() {} 50 | 51 | /** 52 | * @brief A configure state transition to configure navigator's state 53 | * @param node Weakptr to the lifecycle node 54 | */ 55 | bool configure(rclcpp_lifecycle::LifecycleNode::WeakPtr node) override; 56 | 57 | /** 58 | * @brief A cleanup state transition to remove memory allocated 59 | */ 60 | bool cleanup() override; 61 | 62 | /** 63 | * @brief A subscription and callback to handle the topic-based goal published 64 | * from rviz 65 | * @param pose Pose received via atopic 66 | */ 67 | void onGoalPoseReceived( 68 | const geometry_msgs::msg::PoseStamped::SharedPtr pose); 69 | 70 | /** 71 | * @brief Get action name for this navigator 72 | * @return string Name of action server 73 | */ 74 | std::string getName() {return std::string("automatic_recharge");} 75 | 76 | /** 77 | * @brief Get navigator's default BT 78 | * @param node WeakPtr to the lifecycle node 79 | * @return string Filepath to default XML 80 | */ 81 | std::string getDefaultBTFilepath( 82 | rclcpp_lifecycle::LifecycleNode::WeakPtr node) override; 83 | 84 | protected: 85 | /** 86 | * @brief A callback to be called when a new goal is received by the BT action 87 | * server Can be used to check if goal is valid and put values on the 88 | * blackboard which depend on the received goal 89 | * @param goal Action template's goal message 90 | * @return bool if goal was received successfully to be processed 91 | */ 92 | bool goalReceived(ActionT::Goal::ConstSharedPtr goal) override; 93 | 94 | /** 95 | * @brief A callback that defines execution that happens on one iteration 96 | * through the BT Can be used to publish action feedback 97 | */ 98 | void onLoop() override; 99 | 100 | /** 101 | * @brief A callback that is called when a preempt is requested 102 | */ 103 | void onPreempt(ActionT::Goal::ConstSharedPtr goal) override; 104 | 105 | /** 106 | * @brief A callback that is called when a the action is completed, can fill 107 | * in action result message or indicate that this action is done. 108 | * @param result Action template result message to populate 109 | */ 110 | void goalCompleted(typename ActionT::Result::SharedPtr result) override; 111 | 112 | /** 113 | * @brief Goal pose initialization on the blackboard 114 | * @param goal Action template's goal message to process 115 | */ 116 | void initializeGoalPose(ActionT::Goal::ConstSharedPtr goal); 117 | 118 | rclcpp::Time start_time_; 119 | 120 | rclcpp::Subscription::SharedPtr goal_sub_; 121 | rclcpp_action::Client::SharedPtr self_client_; 122 | 123 | std::string charging_station_; 124 | std::string goal_blackboard_keep_dist_; 125 | 126 | // Odometry smoother object 127 | std::unique_ptr odom_smoother_; 128 | 129 | bool onGoalUpdate(FollowPoses::SharedPtr msg); 130 | }; 131 | 132 | } // namespace bt_navigators 133 | 134 | #endif // BT_NAVIGATORS__NAVIGATORS__AUTO_DOCKING_HPP_ 135 | -------------------------------------------------------------------------------- /bt_navigators/include/bt_navigators/navigators/navigate_pose.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Samsung Research 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef BT_NAVIGATORS__NAVIGATORS__NAVIGATE_POSE_HPP_ 16 | #define BT_NAVIGATORS__NAVIGATORS__NAVIGATE_POSE_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "rclcpp_action/rclcpp_action.hpp" 23 | #include "geometry_msgs/msg/pose_stamped.hpp" 24 | #include "bt_navigators/navigator.hpp" 25 | #include "nav2_msgs/action/navigate_to_pose.hpp" 26 | #include "nav2_util/geometry_utils.hpp" 27 | #include "nav2_util/robot_utils.hpp" 28 | #include "nav_msgs/msg/path.hpp" 29 | #include "nav2_util/odometry_utils.hpp" 30 | 31 | namespace bt_navigators 32 | { 33 | 34 | /** 35 | * @class NavigateToPoseNavigator 36 | * @brief A navigator for navigating to a specified pose 37 | */ 38 | class NavABNavigator 39 | : public bt_navigators::Navigator 40 | { 41 | public: 42 | using ActionT = nav2_msgs::action::NavigateToPose; 43 | 44 | /** 45 | * @brief A constructor for NavABNavigator 46 | */ 47 | NavABNavigator() 48 | : Navigator() {} 49 | 50 | /** 51 | * @brief A configure state transition to configure navigator's state 52 | * @param node Weakptr to the lifecycle node 53 | */ 54 | bool configure( 55 | rclcpp_lifecycle::LifecycleNode::WeakPtr node) override; 56 | 57 | /** 58 | * @brief A cleanup state transition to remove memory allocated 59 | */ 60 | bool cleanup() override; 61 | 62 | /** 63 | * @brief A subscription and callback to handle the topic-based goal published 64 | * from rviz 65 | * @param pose Pose received via atopic 66 | */ 67 | void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose); 68 | 69 | /** 70 | * @brief Get action name for this navigator 71 | * @return string Name of action server 72 | */ 73 | std::string getName() {return std::string("navigate_to_pose");} 74 | 75 | /** 76 | * @brief Get navigator's default BT 77 | * @param node WeakPtr to the lifecycle node 78 | * @return string Filepath to default XML 79 | */ 80 | std::string getDefaultBTFilepath(rclcpp_lifecycle::LifecycleNode::WeakPtr node) override; 81 | 82 | protected: 83 | /** 84 | * @brief A callback to be called when a new goal is received by the BT action server 85 | * Can be used to check if goal is valid and put values on 86 | * the blackboard which depend on the received goal 87 | * @param goal Action template's goal message 88 | * @return bool if goal was received successfully to be processed 89 | */ 90 | bool goalReceived(ActionT::Goal::ConstSharedPtr goal) override; 91 | 92 | /** 93 | * @brief A callback that defines execution that happens on one iteration through the BT 94 | * Can be used to publish action feedback 95 | */ 96 | void onLoop() override; 97 | 98 | /** 99 | * @brief A callback that is called when a preempt is requested 100 | */ 101 | void onPreempt(ActionT::Goal::ConstSharedPtr goal) override; 102 | 103 | /** 104 | * @brief A callback that is called when a the action is completed, can fill in 105 | * action result message or indicate that this action is done. 106 | * @param result Action template result message to populate 107 | */ 108 | void goalCompleted(typename ActionT::Result::SharedPtr result) override; 109 | 110 | /** 111 | * @brief Goal pose initialization on the blackboard 112 | * @param goal Action template's goal message to process 113 | */ 114 | void initializeGoalPose(ActionT::Goal::ConstSharedPtr goal); 115 | 116 | bool onGoalUpdate(FollowPoses::SharedPtr msg) override; 117 | 118 | rclcpp::Time start_time_; 119 | 120 | rclcpp::Subscription::SharedPtr goal_sub_; 121 | rclcpp_action::Client::SharedPtr self_client_; 122 | 123 | std::string goal_blackboard_id_; 124 | std::string path_blackboard_id_; 125 | 126 | // Odometry smoother object 127 | std::unique_ptr odom_smoother_; 128 | }; 129 | 130 | } // namespace bt_navigators 131 | 132 | #endif // BT_NAVIGATORS__NAVIGATORS__NAVIGATE_POSE_HPP_ 133 | -------------------------------------------------------------------------------- /bt_navigators/include/bt_navigators/navigators/target_tracking.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Samsung Research 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef BT_NAVIGATORS__NAVIGATORS__TARGET_TRACKING_HPP_ 16 | #define BT_NAVIGATORS__NAVIGATORS__TARGET_TRACKING_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include "rclcpp/rclcpp.hpp" 22 | #include "rclcpp_action/rclcpp_action.hpp" 23 | #include "geometry_msgs/msg/pose_stamped.hpp" 24 | #include "bt_navigators/navigator.hpp" 25 | #include "mcr_msgs/action/target_tracking.hpp" 26 | #include "nav2_util/geometry_utils.hpp" 27 | #include "nav2_util/robot_utils.hpp" 28 | #include "nav_msgs/msg/path.hpp" 29 | #include "nav2_util/odometry_utils.hpp" 30 | 31 | namespace bt_navigators 32 | { 33 | 34 | /** 35 | * @class TargetTrackingNavigator 36 | * @brief A navigator for tracking target 37 | */ 38 | class TargetTrackingNavigator 39 | : public bt_navigators::Navigator 40 | { 41 | public: 42 | using ActionT = mcr_msgs::action::TargetTracking; 43 | 44 | /** 45 | * @brief A constructor for TargetTrackingNavigator 46 | */ 47 | TargetTrackingNavigator() 48 | : Navigator() {} 49 | 50 | /** 51 | * @brief A configure state transition to configure navigator's state 52 | * @param node Weakptr to the lifecycle node 53 | */ 54 | bool configure( 55 | rclcpp_lifecycle::LifecycleNode::WeakPtr node) override; 56 | 57 | /** 58 | * @brief A cleanup state transition to remove memory allocated 59 | */ 60 | bool cleanup() override; 61 | 62 | /** 63 | * @brief A subscription and callback to handle the topic-based goal published 64 | * from rviz 65 | * @param pose Pose received via atopic 66 | */ 67 | void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose); 68 | 69 | /** 70 | * @brief Get action name for this navigator 71 | * @return string Name of action server 72 | */ 73 | std::string getName() {return std::string("tracking_target");} 74 | 75 | /** 76 | * @brief Get navigator's default BT 77 | * @param node WeakPtr to the lifecycle node 78 | * @return string Filepath to default XML 79 | */ 80 | std::string getDefaultBTFilepath(rclcpp_lifecycle::LifecycleNode::WeakPtr node) override; 81 | 82 | protected: 83 | /** 84 | * @brief A callback to be called when a new goal is received by the BT action server 85 | * Can be used to check if goal is valid and put values on 86 | * the blackboard which depend on the received goal 87 | * @param goal Action template's goal message 88 | * @return bool if goal was received successfully to be processed 89 | */ 90 | bool goalReceived(ActionT::Goal::ConstSharedPtr goal) override; 91 | 92 | /** 93 | * @brief A callback that defines execution that happens on one iteration through the BT 94 | * Can be used to publish action feedback 95 | */ 96 | void onLoop() override; 97 | 98 | /** 99 | * @brief A callback that is called when a preempt is requested 100 | */ 101 | void onPreempt(ActionT::Goal::ConstSharedPtr goal) override; 102 | 103 | /** 104 | * @brief A callback that is called when a the action is completed, can fill in 105 | * action result message or indicate that this action is done. 106 | * @param result Action template result message to populate 107 | */ 108 | void goalCompleted(typename ActionT::Result::SharedPtr result) override; 109 | 110 | /** 111 | * @brief Goal pose initialization on the blackboard 112 | * @param goal Action template's goal message to process 113 | */ 114 | void initializeGoalPose(ActionT::Goal::ConstSharedPtr goal); 115 | 116 | rclcpp::Time start_time_; 117 | 118 | rclcpp::Subscription::SharedPtr goal_sub_; 119 | rclcpp_action::Client::SharedPtr self_client_; 120 | 121 | std::string goal_blackboard_relative_pos_; 122 | std::string goal_blackboard_keep_dist_; 123 | std::string target_topic_; 124 | double maxwait_; 125 | geometry_msgs::msg::PoseStamped latest_goal_; 126 | // Odometry smoother object 127 | std::unique_ptr odom_smoother_; 128 | 129 | bool onGoalUpdate(FollowPoses::SharedPtr msg); 130 | }; 131 | 132 | } // namespace bt_navigators 133 | 134 | #endif // BT_NAVIGATORS__NAVIGATORS__TARGET_TRACKING_HPP_ 135 | -------------------------------------------------------------------------------- /bt_navigators/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | bt_navigators 5 | 1.0.8 6 | TODO 7 | Michael Jeronimo 8 | Apache-2.0 9 | 10 | tf2_ros 11 | 12 | ament_cmake 13 | nav2_common 14 | rclcpp 15 | rclcpp_action 16 | rclcpp_lifecycle 17 | nav2_behavior_tree 18 | nav_msgs 19 | nav2_msgs 20 | mcr_msgs 21 | behaviortree_cpp_v3 22 | std_msgs 23 | geometry_msgs 24 | std_srvs 25 | nav2_util 26 | nav2_core 27 | protocol 28 | cyberdog_common 29 | cyberdog_debug 30 | 31 | behaviortree_cpp_v3 32 | rclcpp 33 | rclcpp_action 34 | rclcpp_lifecycle 35 | nav2_behavior_tree 36 | nav_msgs 37 | nav2_msgs 38 | mcr_msgs 39 | std_msgs 40 | nav2_util 41 | geometry_msgs 42 | nav2_core 43 | protocol 44 | ament_lint_common 45 | ament_lint_auto 46 | cyberdog_common 47 | cyberdog_debug 48 | 49 | 50 | ament_cmake 51 | 52 | 53 | -------------------------------------------------------------------------------- /mcr_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mcr_bringup) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(nav2_common REQUIRED) 6 | find_package(navigation2 REQUIRED) 7 | 8 | nav2_package() 9 | 10 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 11 | install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) 12 | install(DIRECTORY behavior_trees DESTINATION share/${PROJECT_NAME}) 13 | 14 | if(BUILD_TESTING) 15 | find_package(ament_lint_auto REQUIRED) 16 | ament_lint_auto_find_test_dependencies() 17 | endif() 18 | 19 | ament_package() 20 | -------------------------------------------------------------------------------- /mcr_bringup/README.md: -------------------------------------------------------------------------------- 1 | # mcr_bringup 2 | 3 | ``` 4 | . 5 | ├── behavior_trees 6 | │   ├── auto_docking.xml 7 | │   ├── automatic_recharge_old.xml 8 | │   ├── automatic_recharge.xml 9 | │   ├── navigate_to_pose_w_replanning_and_recovery.xml 10 | │   └── target_tracking.xml 11 | ├── CMakeLists.txt 12 | ├── launch 13 | │   ├── bringup_dock_launch.py 14 | │   ├── bringup_follow_launch.py 15 | │   ├── bringup_follow_only_launch.py 16 | │   └── bringup_navigate_to_pose_launch.py 17 | ├── package.xml 18 | ├── params 19 | │   ├── auto_charging.yaml 20 | │   ├── auto_docking.yaml 21 | │   ├── follow_params.yaml 22 | │   ├── follow_params.yaml.bak 23 | │   ├── nav2_params.yaml 24 | │   ├── navigate_to_pose_params.yaml 25 | │   └── recoveries_params.yaml 26 | └── README.md 27 | ``` 28 | 29 | ## behavior_trees 30 | 31 | - auto_docking.xml 为自主回充的行为树 32 | - navigate_to_pose_w_replanning_and_recovery.xml 位导航的行为树，继承自nav2_bt_navigator 33 | - target_tracking.xml 位跟随功能的行为树 34 | 35 | ## params 36 | 37 | - auto_docking.yaml 为自主回充配置 38 | - follow_params.yaml 为跟随的配置 39 | - navigate_to_pose_params.yaml 为导航配置 40 | - recoveries_params.yaml 为恢复行为相关配置，为以上三个功能通用 41 | -------------------------------------------------------------------------------- /mcr_bringup/behavior_trees/auto_docking.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /mcr_bringup/behavior_trees/automatic_recharge.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /mcr_bringup/behavior_trees/automatic_recharge_old.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /mcr_bringup/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /mcr_bringup/behavior_trees/target_tracking.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /mcr_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mcr_bringup 5 | 1.0.8 6 | Bringup scripts and configurations for the Nav2 stack 7 | Michael Jeronimo 8 | Steve Macenski 9 | Carlos Orduno 10 | Apache-2.0 11 | 12 | ament_cmake 13 | nav2_common 14 | 15 | navigation2 16 | launch_ros 17 | 18 | launch_ros 19 | navigation2 20 | nav2_common 21 | slam_toolbox 22 | 23 | ament_lint_common 24 | ament_lint_auto 25 | ament_cmake_gtest 26 | ament_cmake_pytest 27 | launch 28 | launch_testing 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /mcr_bringup/params/recoveries_params.yaml: -------------------------------------------------------------------------------- 1 | recoveries_server: 2 | ros__parameters: 3 | costmap_topic: local_costmap/costmap_raw 4 | footprint_topic: local_costmap/published_footprint 5 | cycle_frequency: 10.0 6 | recovery_plugins: ["spin", "spin_and_search", "backup", "wait"] 7 | spin: 8 | plugin: "nav2_recoveries/Spin" 9 | spin_and_search: 10 | plugin: "mcr_tracking_components/SpinAndSearch" 11 | max_rotational_vel: 0.2 12 | min_rotational_vel: 0.1 13 | backup: 14 | plugin: "nav2_recoveries/BackUp" 15 | wait: 16 | plugin: "nav2_recoveries/Wait" 17 | global_frame: vodom 18 | robot_base_frame: base_link 19 | transform_timeout: 0.1 20 | use_sim_time: False 21 | simulate_ahead_time: 2.0 22 | max_rotational_vel: 0.2 23 | min_rotational_vel: 0.1 24 | rotational_acc_lim: 3.2 25 | 26 | 27 | -------------------------------------------------------------------------------- /mcr_controller/.gitignore: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /mcr_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mcr_controller) 3 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 4 | add_compile_options(-Wall -Wextra -Wpedantic -g -O0) 5 | endif() 6 | find_package(ament_cmake REQUIRED) 7 | find_package(nav2_core REQUIRED) 8 | find_package(nav2_common REQUIRED) 9 | find_package(angles REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(rclcpp_action REQUIRED) 12 | find_package(std_msgs REQUIRED) 13 | find_package(std_srvs REQUIRED) 14 | find_package(nav2_util REQUIRED) 15 | find_package(nav2_msgs REQUIRED) 16 | find_package(nav_2d_utils REQUIRED) 17 | find_package(nav_2d_msgs REQUIRED) 18 | find_package(pluginlib REQUIRED) 19 | find_package(cyberdog_debug REQUIRED) 20 | find_package(cyberdog_common REQUIRED) 21 | nav2_package() 22 | 23 | include_directories( 24 | include 25 | ) 26 | 27 | set(executable_name controller_server) 28 | 29 | add_executable(${executable_name} 30 | src/main.cpp 31 | ) 32 | 33 | set(library_name ${executable_name}_core) 34 | 35 | add_library(${library_name} 36 | src/controller_server.cpp 37 | ) 38 | 39 | target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 40 | 41 | set(dependencies 42 | angles 43 | rclcpp 44 | rclcpp_action 45 | std_msgs 46 | std_srvs 47 | nav2_msgs 48 | nav_2d_utils 49 | nav_2d_msgs 50 | nav2_util 51 | nav2_core 52 | pluginlib 53 | cyberdog_debug 54 | cyberdog_common 55 | ) 56 | 57 | ament_target_dependencies(${library_name} 58 | ${dependencies} 59 | ) 60 | 61 | # prevent pluginlib from using boost 62 | target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 63 | 64 | ament_target_dependencies(${executable_name} 65 | ${dependencies} 66 | ) 67 | 68 | target_link_libraries(${executable_name} ${library_name}) 69 | 70 | install(TARGETS ${library_name} 71 | ARCHIVE DESTINATION lib 72 | LIBRARY DESTINATION lib 73 | RUNTIME DESTINATION bin 74 | ) 75 | 76 | install(TARGETS ${executable_name} 77 | RUNTIME DESTINATION lib/${PROJECT_NAME} 78 | ) 79 | 80 | install(DIRECTORY include/ 81 | DESTINATION include/ 82 | ) 83 | 84 | if(BUILD_TESTING) 85 | find_package(ament_lint_auto REQUIRED) 86 | ament_lint_auto_find_test_dependencies() 87 | endif() 88 | 89 | ament_export_include_directories(include) 90 | ament_export_libraries(${library_name}) 91 | ament_export_dependencies(${dependencies}) 92 | 93 | ament_package() 94 | -------------------------------------------------------------------------------- /mcr_controller/README.md: -------------------------------------------------------------------------------- 1 | # Nav2 Controller 2 | 3 | The Nav2 Controller is an [Execution Module](../doc/requirements/requirements.md) that implements the `nav2_msgs::action::FollowPath` action server. 4 | 5 | An execution module implementing the `nav2_msgs::action::FollowPath` action server is responsible for generating command velocities for the robot, given the computed path from the planner module in `nav2_planner`. The nav2_controller package is designed to be loaded with plugins for path execution. The plugins need to implement functions in the virtual base class defined in the `controller` header file in `nav2_core` package. 6 | 7 | 8 | Currently available controller plugins are: DWB, and [TEB (dashing release)](https://github.com/rst-tu-dortmund/teb_local_planner/tree/dashing-devel). -------------------------------------------------------------------------------- /mcr_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mcr_controller 5 | 1.0.8 6 | Controller action interface 7 | zhangchuanfa 8 | Apache-2.0 9 | 10 | ament_cmake 11 | nav2_common 12 | angles 13 | rclcpp 14 | rclcpp_action 15 | std_msgs 16 | std_srvs 17 | nav2_util 18 | nav2_msgs 19 | nav_2d_utils 20 | nav_2d_msgs 21 | nav2_core 22 | pluginlib 23 | cyberdog_debug 24 | cyberdog_common 25 | 26 | ament_lint_common 27 | ament_lint_auto 28 | ament_cmake_gtest 29 | ament_cmake_pytest 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | -------------------------------------------------------------------------------- /mcr_controller/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel Corporation 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 | #include 17 | #include "cyberdog_debug/backtrace.hpp" 18 | #include "mcr_controller/controller_server.hpp" 19 | #include "rclcpp/rclcpp.hpp" 20 | 21 | int main(int argc, char ** argv) 22 | { 23 | rclcpp::init(argc, argv); 24 | cyberdog::debug::register_signal(); 25 | auto node = std::make_shared(); 26 | rclcpp::spin(node->get_node_base_interface()); 27 | rclcpp::shutdown(); 28 | 29 | return 0; 30 | } 31 | -------------------------------------------------------------------------------- /mcr_global_planner/.gitignore: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /mcr_global_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mcr_global_planner) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic -Wno-error=register) 16 | endif() 17 | 18 | # add_compile_options(-g) 19 | 20 | # find dependencies 21 | find_package(ament_cmake REQUIRED) 22 | find_package(rclcpp REQUIRED) 23 | find_package(rclcpp_lifecycle REQUIRED) 24 | find_package(geometry_msgs REQUIRED) 25 | find_package(nav2_core REQUIRED) 26 | find_package(nav2_util REQUIRED) 27 | find_package(pluginlib REQUIRED) 28 | find_package(tf2_ros REQUIRED) 29 | find_package(nav2_costmap_2d REQUIRED) 30 | find_package(nav_2d_msgs REQUIRED) 31 | find_package(nav_2d_utils REQUIRED) 32 | find_package(ompl REQUIRED) 33 | 34 | set(library_name "mcr_global_planner") 35 | add_library(${library_name} SHARED 36 | src/mcr_spline_planner.cpp) 37 | 38 | target_include_directories(${library_name} PUBLIC 39 | ${OMPL_INCLUDE_DIRS} 40 | $ 41 | $) 42 | ament_target_dependencies( 43 | ${library_name} 44 | "rclcpp" 45 | "geometry_msgs" 46 | "nav2_core" 47 | "nav2_util" 48 | "pluginlib" 49 | "tf2_ros" 50 | "nav2_costmap_2d" 51 | "nav_2d_msgs" 52 | "nav_2d_utils" 53 | "ompl" 54 | ) 55 | target_link_libraries(${library_name} ${OMPL_LIBRARIES}) 56 | 57 | # prevent pluginlib from using boost 58 | target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 59 | pluginlib_export_plugin_description_file(nav2_core mcr_global_planner_plugin.xml) 60 | 61 | install(TARGETS ${library_name} 62 | ARCHIVE DESTINATION lib 63 | LIBRARY DESTINATION lib 64 | RUNTIME DESTINATION bin) 65 | 66 | install(DIRECTORY launch 67 | DESTINATION share/${PROJECT_NAME}) 68 | 69 | install(DIRECTORY params 70 | DESTINATION share/${PROJECT_NAME}) 71 | 72 | install(DIRECTORY include/ 73 | DESTINATION include) 74 | 75 | if(BUILD_TESTING) 76 | find_package(ament_lint_auto REQUIRED) 77 | 78 | # the following line skips the linter which checks for copyrights 79 | # uncomment the line when a copyright and license is not present in all source files 80 | # set(ament_cmake_copyright_FOUND TRUE) 81 | # the following line skips cpplint (only works in a git repo) 82 | # uncomment the line when this package is not in a git repo 83 | # set(ament_cmake_cpplint_FOUND TRUE) 84 | ament_lint_auto_find_test_dependencies() 85 | endif() 86 | 87 | ament_export_include_directories(include) 88 | ament_export_libraries(${library_name}) 89 | ament_export_dependencies(${dependencies}) 90 | ament_export_definitions("PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 91 | 92 | ament_package() 93 | -------------------------------------------------------------------------------- /mcr_global_planner/include/mcr_global_planner/mcr_spline_planner.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MCR_GLOBAL_PLANNER__MCR_SPLINE_PLANNER_HPP_ 16 | #define MCR_GLOBAL_PLANNER__MCR_SPLINE_PLANNER_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include "nav2_core/global_planner.hpp" 22 | #include "nav2_costmap_2d/costmap_2d_ros.hpp" 23 | 24 | #include "nav_2d_msgs/msg/pose2_d_stamped.hpp" 25 | #include "geometry_msgs/msg/pose_array.hpp" 26 | #include "pluginlib/class_loader.hpp" 27 | #include "rclcpp/rclcpp.hpp" 28 | 29 | namespace mcr_global_planner 30 | { 31 | class Spliner; 32 | 33 | class MCRSplinePlanner : public nav2_core::GlobalPlanner 34 | { 35 | public: 36 | MCRSplinePlanner(); 37 | ~MCRSplinePlanner() 38 | { 39 | } 40 | 41 | /** 42 | * @brief Configuring plugin 43 | * @param parent Lifecycle node pointer 44 | * @param name Name of plugin map 45 | * @param tf Shared ptr of TF2 buffer 46 | * @param costmap_ros Costmap2DROS object 47 | */ 48 | void configure( 49 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, 50 | std::string name, std::shared_ptr tf, 51 | std::shared_ptr costmap_ros); 52 | 53 | /** 54 | * @brief Cleanup lifecycle node 55 | */ 56 | void cleanup() override; 57 | 58 | /** 59 | * @brief Activate lifecycle node 60 | */ 61 | void activate() override; 62 | 63 | /** 64 | * @brief Deactivate lifecycle node 65 | */ 66 | void deactivate() override; 67 | 68 | 69 | /** 70 | * @brief Creating a plan from start and goal poses 71 | * @param start Start pose 72 | * @param goal Goal pose 73 | * @return nav_msgs::Path of the generated path 74 | */ 75 | nav_msgs::msg::Path createPlan( 76 | const geometry_msgs::msg::PoseStamped & start, 77 | const geometry_msgs::msg::PoseStamped & goal) override; 78 | nav_msgs::msg::Path createPlan( 79 | const std::vector & poses) override; 80 | 81 | // virtual bool isPlanValid(const nav_msgs::msg::Path & path) const; 82 | nav_msgs::msg::Path spline( 83 | const std::vector& poses); 84 | std::vector&& interpolation( 85 | const std::vector& control_points); 86 | protected: 87 | nav2_util::LifecycleNode::SharedPtr node_; 88 | std::string global_frame_, name_; 89 | std::shared_ptr tf_; 90 | std::shared_ptr costmap_ros_; 91 | std::shared_ptr costmap_; 92 | 93 | std::string spliner_name_; 94 | rclcpp::TimerBase::SharedPtr timer_; 95 | }; 96 | } // namespace mcr_global_planner 97 | 98 | #endif // MCR_GLOBAL_PLANNER__MCR_SPLINE_PLANNER_HPP_ 99 | -------------------------------------------------------------------------------- /mcr_global_planner/launch/mcr_planner.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2020 Samsung Research Russia 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import os 18 | from launch import LaunchDescription 19 | import launch_ros.actions 20 | from nav2_common.launch import RewrittenYaml 21 | from ament_index_python.packages import get_package_share_directory 22 | 23 | 24 | def generate_launch_description(): 25 | # Parameters 26 | lifecycle_nodes = ['mcr_planner'] 27 | use_sim_time = True 28 | autostart = True 29 | package_dir = get_package_share_directory("mcr_global_planner") 30 | planner_param_file = os.path.join(package_dir, "params/planner_params.yaml") 31 | 32 | configured_params = RewrittenYaml( 33 | source_file=planner_param_file, 34 | param_rewrites={} 35 | ) 36 | 37 | # Nodes launching commands 38 | planner_cmd = launch_ros.actions.Node( 39 | package='mcr_global_planner', 40 | executable='global_planner', 41 | name='mcr_planner', 42 | output='screen', 43 | emulate_tty=True, # https://github.com/ros2/launch/issues/188 44 | parameters=[configured_params], 45 | prefix=['xterm -e gdb --args'] 46 | ) 47 | 48 | start_lifecycle_manager_cmd = launch_ros.actions.Node( 49 | package='nav2_lifecycle_manager', 50 | executable='lifecycle_manager', 51 | name='lifecycle_manager', 52 | output='screen', 53 | emulate_tty=True, # https://github.com/ros2/launch/issues/188 54 | parameters=[{'use_sim_time': use_sim_time}, 55 | {'autostart': autostart}, 56 | {'node_names': lifecycle_nodes}]) 57 | 58 | ld = LaunchDescription() 59 | 60 | ld.add_action(planner_cmd) 61 | ld.add_action(start_lifecycle_manager_cmd) 62 | 63 | return ld 64 | -------------------------------------------------------------------------------- /mcr_global_planner/mcr_global_planner_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /mcr_global_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mcr_global_planner 5 | 0.0.0 6 | TODO: Package description 7 | zhangchuanfa 8 | Apache License, Version 2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_lifecycle 14 | geometry_msgs 15 | nav2_core 16 | nav2_util 17 | pluginlib 18 | tf2_ros 19 | nav2_costmap_2d 20 | nav_2d_msgs 21 | nav_2d_utils 22 | ompl 23 | 24 | ament_lint_auto 25 | ament_lint_common 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /mcr_global_planner/params/planner_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_costmap: 3 | ros__parameters: 4 | update_frequency: 1.0 5 | publish_frequency: 1.0 6 | global_frame: odom 7 | robot_base_frame: base_link 8 | rolling_window: true 9 | use_sim_time: True 10 | width: 10 11 | height: 10 12 | robot_radius: 0.22 13 | resolution: 0.05 14 | track_unknown_space: false 15 | plugins: ["obstacle_layer", "inflation_layer"] 16 | obstacle_layer: 17 | plugin: "mcr_tracking_components::ObstacleLayer" 18 | enabled: True 19 | use_tracking_mode: True 20 | tracking_pose_invisible_radius: 0.8 21 | tracking_pose_topic: tracking_pose 22 | observation_sources: scan 23 | scan: 24 | topic: /scan 25 | max_obstacle_height: 10.0 26 | min_obstacle_height: -10.0 27 | clearing: True 28 | marking: True 29 | data_type: "LaserScan" 30 | raytrace_max_range: 6.0 31 | raytrace_min_range: 0.0 32 | obstacle_max_range: 5.5 33 | obstacle_min_range: 0.0 34 | static_layer: 35 | plugin: "nav2_costmap_2d::StaticLayer" 36 | map_subscribe_transient_local: True 37 | inflation_layer: 38 | plugin: "nav2_costmap_2d::InflationLayer" 39 | cost_scaling_factor: 3.0 40 | inflation_radius: 0.55 41 | always_send_full_costmap: True 42 | rolling_window_costmap_client: 43 | ros__parameters: 44 | use_sim_time: True 45 | rolling_window_costmap_rclcpp_node: 46 | ros__parameters: 47 | use_sim_time: True 48 | 49 | 50 | mcr_planner: 51 | ros__parameters: 52 | use_sim_time: True 53 | planner_name: potential_planner 54 | potential_planner: 55 | use_sim_time: True 56 | plugin: mcr_global_planner/MCRGlobalPlanner 57 | potential_calculator: mcr_planner_plugins::AStar #Dijkstra 58 | traceback: mcr_planner_plugins::GradientPath #GridPath #VonNeumannPath 59 | publish_potential: false 60 | print_statistics: false 61 | neutral_cost: 50 62 | scale: 3.0 63 | path_caching: true 64 | improvement_threshold: -1.0 65 | use_kernel: true 66 | unknown_interpretation: lethal 67 | 68 | curve_planner: 69 | use_sim_time: True 70 | plugin: mcr_global_planner::MCRCurvePlanner 71 | curve_type: dubs 72 | turning_radius: 1.5 73 | 74 | spline_planner: 75 | use_sim_time: True 76 | plugin: mcr_global_planner::MCRSplinePlanner 77 | spline_name: bezier 78 | pose_topic: "goal_pose1" 79 | bezier: 80 | plugin: mcr_planner_plugins::PolynomialFitting 81 | # plugin: mcr_planner_plugins::BSpliner 82 | # plugin: mcr_planner_plugins::PolynomialInterpolation 83 | # plugin: mcr_planner_plugins::BezierSpliner 84 | 85 | grid_base: 86 | plugin: "astar_planner/AstarPlanner" 87 | tolerance: 0.5 88 | use_astar: false 89 | allow_unknown: true 90 | -------------------------------------------------------------------------------- /mcr_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mcr_msgs) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(nav2_common REQUIRED) 6 | find_package(builtin_interfaces REQUIRED) 7 | find_package(nav_msgs REQUIRED) 8 | find_package(geometry_msgs REQUIRED) 9 | find_package(rosidl_default_generators REQUIRED) 10 | find_package(std_msgs REQUIRED) 11 | find_package(action_msgs REQUIRED) 12 | 13 | nav2_package() 14 | 15 | rosidl_generate_interfaces(${PROJECT_NAME} 16 | "srv/SwitchMode.srv" 17 | "action/ComputePathSplinePoses.action" 18 | "action/TargetTracking.action" 19 | "action/AutomaticRecharge.action" 20 | DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs 21 | ) 22 | 23 | ament_export_dependencies(rosidl_default_runtime) 24 | 25 | ament_package() 26 | -------------------------------------------------------------------------------- /mcr_msgs/README.md: -------------------------------------------------------------------------------- 1 | # mcr_msgs 2 | 3 | 跟随相关的消息格式定义如下： 4 | 5 | ``` 6 | # 自主选择跟随位置 7 | uint8 AUTO = 0 8 | # 在目标后侧跟随 9 | uint8 BEHIND = 1 10 | # 在目标的左侧跟随 11 | uint8 LEFT = 2 12 | # 在目标的右侧跟随 13 | uint8 RIGHT = 3 14 | 15 | #goal definition 16 | # 相对方位，以上方定义的枚举值指定 17 | uint8 relative_pos 18 | # 与跟随目标所保持的距离 19 | float32 keep_distance 20 | # 行为树的名字（预留） 21 | string behavior_tree 22 | --- 23 | #result definition 24 | # 结果（预留） 25 | std_msgs/Empty result 26 | --- 27 | # 当前与目标之间的实际距离 28 | float32 current_distance 29 | # 跟随当前目标总计时间 30 | builtin_interfaces/Duration tracking_time 31 | # 经历过异常自恢复的次数 32 | int16 number_of_recoveries 33 | # 异常码 34 | int16 exception_code 35 | ``` 36 | -------------------------------------------------------------------------------- /mcr_msgs/action/AutomaticRecharge.action: -------------------------------------------------------------------------------- 1 | 2 | # 行为树的名字（预留） 3 | string behavior_tree 4 | --- 5 | #result definition 6 | # 结果（预留） 7 | std_msgs/Empty result 8 | --- 9 | # 当前与目标之间的实际距离 10 | float32 current_distance 11 | # 跟随当前目标总计时间 12 | builtin_interfaces/Duration tracking_time 13 | # 经历过异常自恢复的次数 14 | int16 number_of_recoveries 15 | # 异常码 16 | int16 exception_code 17 | -------------------------------------------------------------------------------- /mcr_msgs/action/ComputePathSplinePoses.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | geometry_msgs/PoseStamped[] poses 3 | string planner_id 4 | --- 5 | #result definition 6 | nav_msgs/Path path 7 | builtin_interfaces/Duration planning_time 8 | --- 9 | #feedback 10 | -------------------------------------------------------------------------------- /mcr_msgs/action/TargetTracking.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | uint8 NAVIGATION_TYPE_START_UWB_TRACKING = 11 3 | uint8 NAVIGATION_TYPE_STOP_UWB_TRACKING = 12 4 | uint8 NAVIGATION_TYPE_START_HUMAN_TRACKING = 13 5 | uint8 NAVIGATION_TYPE_STOP_HUMAN_TRACKING = 14 6 | 7 | uint8 nav_type 8 | # 自主选择跟随位置 9 | uint8 AUTO = 0 10 | # 在目标后侧跟随 11 | uint8 BEHIND = 1 12 | # 在目标的左侧跟随 13 | uint8 LEFT = 2 14 | # 在目标的右侧跟随 15 | uint8 RIGHT = 3 16 | 17 | # 相对方位，以上方定义的枚举值指定 18 | uint8 relative_pos 19 | # 与跟随目标所保持的距离 20 | float32 keep_distance 21 | # 行为树的名字（预留） 22 | string behavior_tree 23 | --- 24 | #result definition 25 | # 结果（预留） 26 | std_msgs/Empty result 27 | --- 28 | # 当前与目标之间的实际距离 29 | float32 current_distance 30 | # 跟随当前目标总计时间 31 | builtin_interfaces/Duration tracking_time 32 | # 经历过异常自恢复的次数 33 | int16 number_of_recoveries 34 | # 异常码 35 | int16 exception_code 36 | # 与目标保持的距离 37 | float32 keep_distance 38 | # 当前档位的速度极限 39 | float32 max_x 40 | # 当前运动的状态 41 | int16 motion_state 42 | -------------------------------------------------------------------------------- /mcr_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mcr_msgs 5 | 1.0.8 6 | Messages and service files for the Nav2 stack 7 | Michael Jeronimo 8 | Steve Macenski 9 | Carlos Orduno 10 | Apache-2.0 11 | 12 | ament_cmake 13 | nav2_common 14 | 15 | rclcpp 16 | std_msgs 17 | builtin_interfaces 18 | rosidl_default_generators 19 | geometry_msgs 20 | action_msgs 21 | nav_msgs 22 | 23 | rosidl_interface_packages 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /mcr_msgs/srv/SwitchMode.srv: -------------------------------------------------------------------------------- 1 | # 自主选择跟随位置 2 | uint8 AUTO = 0 3 | # 在目标后侧跟随 4 | uint8 BEHIND = 1 5 | # 在目标的左侧跟随 6 | uint8 LEFT = 2 7 | # 在目标的右侧跟随 8 | uint8 RIGHT = 3 9 | 10 | # 相对方位，以上方定义的枚举值指定 11 | uint8 relative_pos 12 | --- 13 | bool result -------------------------------------------------------------------------------- /mcr_planner/.gitignore: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /mcr_planner/CHANGELOG.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_tracking_base/0bb7dfa9fc22e1c877c3ca753c0a663b3531c814/mcr_planner/CHANGELOG.rst -------------------------------------------------------------------------------- /mcr_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mcr_planner) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(nav2_common REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(rclcpp_action REQUIRED) 8 | find_package(rclcpp_lifecycle REQUIRED) 9 | find_package(std_msgs REQUIRED) 10 | find_package(visualization_msgs REQUIRED) 11 | find_package(nav2_util REQUIRED) 12 | find_package(nav2_msgs REQUIRED) 13 | find_package(mcr_msgs REQUIRED) 14 | find_package(nav_msgs REQUIRED) 15 | find_package(geometry_msgs REQUIRED) 16 | find_package(builtin_interfaces REQUIRED) 17 | find_package(tf2_ros REQUIRED) 18 | find_package(nav2_costmap_2d REQUIRED) 19 | find_package(pluginlib REQUIRED) 20 | find_package(nav2_core REQUIRED) 21 | find_package(cyberdog_debug REQUIRED) 22 | find_package(cyberdog_common REQUIRED) 23 | nav2_package() 24 | 25 | include_directories( 26 | include 27 | ) 28 | # add_compile_options(-g) 29 | set(executable_name mcr_planner_server) 30 | set(library_name ${executable_name}_core) 31 | 32 | set(dependencies 33 | rclcpp 34 | rclcpp_action 35 | rclcpp_lifecycle 36 | std_msgs 37 | visualization_msgs 38 | nav2_util 39 | nav2_msgs 40 | nav_msgs 41 | mcr_msgs 42 | geometry_msgs 43 | builtin_interfaces 44 | tf2_ros 45 | nav2_costmap_2d 46 | pluginlib 47 | nav2_core 48 | cyberdog_debug 49 | cyberdog_common 50 | ) 51 | 52 | add_library(${library_name} SHARED 53 | src/planner_server.cpp 54 | ) 55 | 56 | ament_target_dependencies(${library_name} 57 | ${dependencies} 58 | ) 59 | 60 | target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 61 | 62 | add_executable(${executable_name} 63 | src/main.cpp 64 | ) 65 | 66 | target_link_libraries(${executable_name} ${library_name}) 67 | 68 | ament_target_dependencies(${executable_name} 69 | ${dependencies} 70 | ) 71 | 72 | # prevent pluginlib from using boost 73 | target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 74 | 75 | install(TARGETS ${library_name} 76 | ARCHIVE DESTINATION lib 77 | LIBRARY DESTINATION lib 78 | RUNTIME DESTINATION bin 79 | ) 80 | 81 | install(TARGETS ${executable_name} 82 | RUNTIME DESTINATION lib/${PROJECT_NAME} 83 | ) 84 | 85 | install(DIRECTORY include/ 86 | DESTINATION include/ 87 | ) 88 | 89 | if(BUILD_TESTING) 90 | find_package(ament_lint_auto REQUIRED) 91 | ament_lint_auto_find_test_dependencies() 92 | endif() 93 | 94 | ament_export_include_directories(include) 95 | ament_export_libraries(${library_name}) 96 | ament_export_dependencies(${dependencies}) 97 | ament_package() 98 | -------------------------------------------------------------------------------- /mcr_planner/README.md: -------------------------------------------------------------------------------- 1 | # Mcr Planner 2 | 3 | -------------------------------------------------------------------------------- /mcr_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mcr_planner 5 | 1.0.8 6 | TODO 7 | zhangchuanfa 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_action 14 | rclcpp_lifecycle 15 | visualization_msgs 16 | nav2_util 17 | nav2_msgs 18 | mcr_msgs 19 | nav_msgs 20 | geometry_msgs 21 | builtin_interfaces 22 | nav2_common 23 | tf2_ros 24 | nav2_costmap_2d 25 | pluginlib 26 | nav2_core 27 | cyberdog_debug 28 | cyberdog_common 29 | ament_lint_common 30 | ament_lint_auto 31 | 32 | 33 | ament_cmake 34 | 35 | 36 | -------------------------------------------------------------------------------- /mcr_planner/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 2 | // Copyright (c) 2019 Samsung Research America 3 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 4 | // 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | // 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | // 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | 17 | #include 18 | #include "cyberdog_debug/backtrace.hpp" 19 | #include "mcr_planner/planner_server.hpp" 20 | #include "rclcpp/rclcpp.hpp" 21 | 22 | int main(int argc, char ** argv) 23 | { 24 | cyberdog::debug::register_signal(); 25 | rclcpp::init(argc, argv); 26 | auto node = std::make_shared(); 27 | rclcpp::spin(node->get_node_base_interface()); 28 | rclcpp::shutdown(); 29 | 30 | return 0; 31 | } -------------------------------------------------------------------------------- /mcr_tracking_components/.gitignore: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /mcr_tracking_components/README.md: -------------------------------------------------------------------------------- 1 | # mcr_tracking_components 2 | 3 | 主要是基于navigation2中的插件接口进行扩充，主要包括了如下几个种类的插件 4 | 5 | ## 行为树节点 6 | 7 | ### change_gait_node 8 | 9 | 用于调整狗子步态的节点 10 | 11 | ### is_battery_in_charge_condition 12 | 13 | 检测是否已经开始进行充电的节点 14 | 15 | ### charger_updater_node 16 | 17 | 获取自主回充时，充电桩的位置信息 18 | 19 | ### orientation_derivers 20 | 21 | 基于卡尔曼，用于推导目标运行方向的计算 22 | 23 | ### compute_path_spline_poses_action 24 | 25 | 修改自navigation2， 用于跟随 26 | 27 | ### seat_adjust_client_node 28 | 29 | 自主回充时，最后趴下之前，调整姿态的接口 30 | 31 | ### compute_path_to_pose_action 32 | 33 | 修改自navigation2，增加了一些跟随功能专用接口 34 | 35 | ### spin_and_search_action 36 | 37 | 当目标丢失时，通过自旋转来找回目标 38 | 39 | ### exception_verify_node 40 | 41 | 异常校验，用于系统发生不同的异常时匹配不同的恢复行为 42 | 43 | ### target_updater_node 44 | 45 | 目标管理节点，用于目标位置接收和处理 46 | 47 | ### follow_path_action 48 | 49 | 修改自navigation2的follow path节点，增加了一些跟随功能专用接口 50 | 51 | ### tracking_mode_decider_node 52 | 53 | 预留，用于自主决策跟随时狗子相对与目标的位置。 54 | 55 | ## 控制器插件 56 | 57 | ### fake_progress_checker 58 | 59 | 不管机器人是否存在运动，都认为狗子运行正常 60 | 61 | ### x_goal_checker 62 | 63 | 只检测x方向是否抵达 64 | 65 | ### unreachable_goal_checker 66 | 67 | 不管机器人是否抵达目标，都返回未抵达 68 | 69 | ### xy_goal_checker 70 | 71 | 只检测直线距离是否满足阈值，不考虑航向角 72 | 73 | ## costmap的layer插件 74 | 75 | ### obstacle_layer 76 | 77 | 修改自navigation2，订阅目标信息然后在传感器数据中扣除目标。 78 | 79 | 80 | ## dwb规划器critic插件 81 | 82 | ### keep_target_insight 83 | 84 | 为了保持目标能够一直保持在摄像头视野，利用目标数据所增加的dwb评判器 85 | 86 | ### keep_person_insight 87 | 88 | 为了保持目标能够一直保持在摄像头视野，利用目标上游数据（reid结果）所增加的dwb评判器 89 | 90 | ## 恢复行为的插件 91 | 92 | ### spin_and_search 93 | 94 | 当目标丢失时候，会一边旋转一边找回 95 | -------------------------------------------------------------------------------- /mcr_tracking_components/behavior_tree_nodes.xml: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | Destinations to plan splined 12 | Path created by ComputePathToPose node 13 | 14 | 15 | 16 | Destination to plan to 17 | Start pose of the path if overriding current robot pose 18 | Path created by ComputePathToPose node 19 | 20 | 21 | 22 | 23 | 24 | Destination to plan to 25 | Path to follow 26 | Goal checker 27 | Progress checker 28 | Exception code for the real reason 29 | 30 | 31 | 32 | Original tracking mode 33 | Output tracking mode by decision 34 | 35 | 36 | gait to use next. 37 | gait to use next. 38 | gait to use next. 39 | start signal to next action 40 | 41 | 42 | 43 | Spin distance 44 | 45 | 46 | To start the action. 47 | 48 | 49 | Distance before goal to truncate 50 | Path to truncate 51 | Truncated path to utilize 52 | 53 | 54 | 55 | Min battery % or voltage before triggering 56 | Topic for battery info 57 | Bool if check based on voltage or total % 58 | 59 | 60 | 61 | 62 | input code 63 | right code 64 | 65 | 66 | Original goal in 67 | current charing phase 68 | Distance between target and robot. 69 | Output goals set by subscription 70 | Exception code for the real reason 71 | 72 | 73 | 74 | Original goal in 75 | Original tracking mode 76 | Distance between target and robot. 77 | Output goal set by subscription 78 | Output goal set by transformed_goal subscription 79 | Output goals set by subscription 80 | Exception code for the real reason 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /mcr_tracking_components/controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | fake , force true. 6 | 7 | 8 | 9 | 10 | Checks if current pose is within goal window for x,y 11 | 12 | 13 | 14 | 15 | Checks if current pose is within goal window for x,yaw 16 | 17 | 18 | 19 | 20 | the goal will never reached. 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /mcr_tracking_components/costmap_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Listens to laser scan and point cloud messages(without target radius area) and 5 | marks and clears grid cells. 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /mcr_tracking_components/dwb_critics.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Forces the commanded trajectories to towards the target. 5 | 6 | 7 | 8 | Forces the commanded trajectories to towards the person. 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/behavior_tree_nodes/change_gait_node.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef mcr_tracking_components__PLUGINS__ACTION__CHANGE_GAIT_NODE_HPP_ 16 | #define mcr_tracking_components__PLUGINS__ACTION__CHANGE_GAIT_NODE_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "behaviortree_cpp_v3/action_node.h" 22 | #include "nav_msgs/msg/path.hpp" 23 | #include "protocol/srv/motion_result_cmd.hpp" 24 | #include "rclcpp/rclcpp.hpp" 25 | #include "std_msgs/msg/int8.hpp" 26 | #include "nav2_behavior_tree/bt_service_node.hpp" 27 | namespace mcr_tracking_components { 28 | 29 | using MotionServiceT = protocol::srv::MotionResultCmd; 30 | /** 31 | * @brief A BT::BtServiceNode to make decision for tracking mode 32 | */ 33 | class ChangeGait : public nav2_behavior_tree::BtServiceNode { 34 | public: 35 | /** 36 | * @brief A mcr_tracking_components::ChangeGait constructor 37 | * @param name Name for this node 38 | * @param conf BT node configuration 39 | */ 40 | ChangeGait(const std::string& name, 41 | const BT::NodeConfiguration& conf); 42 | 43 | /** 44 | * @brief Creates list of BT ports 45 | * @return BT::PortsList Containing basic ports along with node-specific ports 46 | */ 47 | static BT::PortsList providedPorts() { 48 | return providedBasicPorts( 49 | { 50 | BT::InputPort("gait_motion_id", "the gait for next step."), 51 | BT::InputPort("gait_shape_value", "the gait for next step."), 52 | BT::InputPort("gait_step_height", "the gait for next step."), 53 | BT::OutputPort("next_action_start", 54 | "start signal to next action."), 55 | }); 56 | } 57 | 58 | virtual void on_wait_for_result(); 59 | 60 | /** 61 | * @brief Function to perform some user-defined operation on tick 62 | */ 63 | virtual void on_tick(); 64 | }; 65 | 66 | } // namespace mcr_tracking_components 67 | 68 | #endif // mcr_tracking_components__PLUGINS__ACTION__CHANGE_GAIT_ACTION_HPP_ 69 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/behavior_tree_nodes/charger_updater_node.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef mcr_tracking_components__PLUGINS__DECORATOR__CHARGER_UPDATER_NODE_HPP_ 16 | #define mcr_tracking_components__PLUGINS__DECORATOR__CHARGER_UPDATER_NODE_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "behaviortree_cpp_v3/decorator_node.h" 25 | #include "geometry_msgs/msg/pose_stamped.hpp" 26 | #include "nav_msgs/msg/path.hpp" 27 | #include "rclcpp/rclcpp.hpp" 28 | #include "tf2_ros/buffer.h" 29 | #include "tf2_ros/transform_listener.h" 30 | namespace mcr_tracking_components { 31 | 32 | /** 33 | * @brief A BT::DecoratorNode that subscribes to a goal topic and updates 34 | * the current goal on the blackboard 35 | */ 36 | class ChargerUpdater : public BT::DecoratorNode { 37 | public: 38 | /** 39 | * @brief A constructor for mcr_tracking_components::ChargerUpdater 40 | * @param xml_tag_name Name for the XML tag for this node 41 | * @param conf BT node configuration 42 | */ 43 | ChargerUpdater(const std::string& xml_tag_name, 44 | const BT::NodeConfiguration& conf); 45 | 46 | /** 47 | * @brief Creates list of BT ports 48 | * @return BT::PortsList Containing node-specific ports 49 | */ 50 | static BT::PortsList providedPorts() { 51 | return { 52 | BT::OutputPort>( 53 | "output_goals", "Received Goals by subscription"), 54 | BT::OutputPort("output_exception_code", 55 | "Exception code for the real reason."), 56 | BT::InputPort("phase", "current autocharing phase"), 57 | }; 58 | } 59 | 60 | private: 61 | /** 62 | * @brief The main override required by a BT action 63 | * @return BT::NodeStatus Status of tick execution 64 | */ 65 | BT::NodeStatus tick() override; 66 | 67 | /** 68 | * @brief Callback function for goal update topic 69 | * @param msg Shared pointer to geometry_msgs::msg::PoseStamped message 70 | */ 71 | void callback_updated_goal( 72 | const geometry_msgs::msg::PoseStamped::SharedPtr msg); 73 | void publishPoses(const std::deque& poses); 74 | bool isValid(const geometry_msgs::msg::PoseStamped::SharedPtr msg); 75 | geometry_msgs::msg::PoseStamped translatePose( 76 | const geometry_msgs::msg::PoseStamped& pose); 77 | rclcpp::Subscription::SharedPtr goal_sub_; 78 | rclcpp::Publisher::SharedPtr 79 | transformed_pose_pub_; 80 | geometry_msgs::msg::PoseStamped last_goal_received_; 81 | geometry_msgs::msg::PoseStamped last_goal_transformed_; 82 | rclcpp::Time latest_timestamp_; 83 | 84 | std::mutex mutex_; 85 | 86 | std::string global_frame_; 87 | std::string robot_base_frame_; 88 | std::shared_ptr tf_buffer_; 89 | std::shared_ptr tf_listener_; 90 | std::deque historical_poses_; 91 | int max_pose_inuse_; 92 | float distance_; 93 | float distance_final_; 94 | double offset_x_, offset_y_; 95 | std::deque average_poses_; 96 | rclcpp::Node::SharedPtr node_; 97 | rclcpp::CallbackGroup::SharedPtr callback_group_; 98 | rclcpp::executors::SingleThreadedExecutor callback_group_executor_; 99 | geometry_msgs::msg::PoseStamped final_goal_; 100 | bool final_goal_got_; 101 | }; 102 | 103 | } // namespace mcr_tracking_components 104 | 105 | #endif // mcr_tracking_components__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_ 106 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/behavior_tree_nodes/compute_path_spline_poses_action.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Samsung Research America 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 | #ifndef mcr_tracking_components__PLUGINS__ACTION__COMPUTE_PATH_SPLINE_POSES_ACTION_HPP_ 16 | #define mcr_tracking_components__PLUGINS__ACTION__COMPUTE_PATH_SPLINE_POSES_ACTION_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "mcr_msgs/action/compute_path_spline_poses.hpp" 22 | #include "nav2_behavior_tree/bt_action_node.hpp" 23 | #include "nav_msgs/msg/path.h" 24 | 25 | namespace mcr_tracking_components { 26 | 27 | /** 28 | * @brief A mcr_tracking_components::BtActionNode class that wraps 29 | * nav2_msgs::action::ComputePathSplinePoses 30 | */ 31 | class ComputePathSplinePosesAction 32 | : public nav2_behavior_tree::BtActionNode< 33 | mcr_msgs::action::ComputePathSplinePoses> { 34 | public: 35 | /** 36 | * @brief A constructor for 37 | * mcr_tracking_components::ComputePathSplinePosesAction 38 | * @param xml_tag_name Name for the XML tag for this node 39 | * @param action_name Action name this node creates a client for 40 | * @param conf BT node configuration 41 | */ 42 | ComputePathSplinePosesAction(const std::string& xml_tag_name, 43 | const std::string& action_name, 44 | const BT::NodeConfiguration& conf); 45 | 46 | /** 47 | * @brief Function to perform some user-defined operation on tick 48 | */ 49 | void on_tick() override; 50 | 51 | /** 52 | * @brief Function to perform some user-defined operation upon successful 53 | * completion of the action 54 | */ 55 | BT::NodeStatus on_success() override; 56 | 57 | /** 58 | * @brief Creates list of BT ports 59 | * @return BT::PortsList Containing basic ports along with node-specific ports 60 | */ 61 | static BT::PortsList providedPorts() { 62 | return providedBasicPorts({ 63 | BT::OutputPort( 64 | "path", "Path created by ComputePathThroughPoses node"), 65 | BT::InputPort>( 66 | "poses", "Destinations to plan splined"), 67 | BT::InputPort("planner_id", ""), 68 | }); 69 | } 70 | }; 71 | 72 | } // namespace mcr_tracking_components 73 | 74 | #endif // mcr_tracking_components__PLUGINS__ACTION__COMPUTE_PATH_SPLINE_POSES_ACTION_HPP_ 75 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/behavior_tree_nodes/compute_path_to_pose_action.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 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 | #ifndef MCR_TRACKING_COMPONENTS__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_ 16 | #define MCR_TRACKING_COMPONENTS__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_ 17 | 18 | #include 19 | 20 | #include "nav2_msgs/action/compute_path_to_pose.hpp" 21 | #include "nav_msgs/msg/path.h" 22 | #include "nav2_behavior_tree/bt_action_node.hpp" 23 | 24 | namespace mcr_tracking_components 25 | { 26 | 27 | /** 28 | * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputePathToPose 29 | */ 30 | class ComputePathToPoseAction : public nav2_behavior_tree::BtActionNode 31 | { 32 | public: 33 | /** 34 | * @brief A constructor for nav2_behavior_tree::ComputePathToPoseAction 35 | * @param xml_tag_name Name for the XML tag for this node 36 | * @param action_name Action name this node creates a client for 37 | * @param conf BT node configuration 38 | */ 39 | ComputePathToPoseAction( 40 | const std::string & xml_tag_name, 41 | const std::string & action_name, 42 | const BT::NodeConfiguration & conf); 43 | 44 | /** 45 | * @brief Function to perform some user-defined operation on tick 46 | */ 47 | void on_tick() override; 48 | 49 | /** 50 | * @brief Function to perform some user-defined operation upon successful completion of the action 51 | */ 52 | BT::NodeStatus on_success() override; 53 | 54 | void on_wait_for_result() override; 55 | /** 56 | * @brief Creates list of BT ports 57 | * @return BT::PortsList Containing basic ports along with node-specific ports 58 | */ 59 | static BT::PortsList providedPorts() 60 | { 61 | return providedBasicPorts( 62 | { 63 | BT::OutputPort("path", "Path created by ComputePathToPose node"), 64 | BT::InputPort("goal", "Destination to plan to"), 65 | BT::InputPort( 66 | "start", "Start pose of the path if overriding current robot pose"), 67 | BT::InputPort("planner_id", ""), 68 | }); 69 | } 70 | }; 71 | 72 | } // namespace mcr_tracking_components 73 | 74 | #endif // MCR_TRACKING_COMPONENTS__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_ 75 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/behavior_tree_nodes/exception_verify_node.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef mcr_tracking_components__PLUGINS__DECORATOR__EXCEPTION_VERIFY_NODE_HPP_ 16 | #define mcr_tracking_components__PLUGINS__DECORATOR__EXCEPTION_VERIFY_NODE_HPP_ 17 | 18 | #include 19 | #include 20 | #include "rclcpp/rclcpp.hpp" 21 | #include "behaviortree_cpp_v3/decorator_node.h" 22 | 23 | namespace mcr_tracking_components 24 | { 25 | 26 | /** 27 | * @brief A BT::DecoratorNode that triggers its child only once and returns FAILURE 28 | * for every succeeding tick 29 | */ 30 | class ExceptionVerify : public BT::DecoratorNode 31 | { 32 | public: 33 | /** 34 | * @brief A constructor for mcr_tracking_components::ExceptionVerify 35 | * @param name Name for the XML tag for this node 36 | * @param conf BT node configuration 37 | */ 38 | ExceptionVerify( 39 | const std::string & name, 40 | const BT::NodeConfiguration & conf); 41 | 42 | /** 43 | * @brief Creates list of BT ports 44 | * @return BT::PortsList Containing node-specific ports 45 | */ 46 | static BT::PortsList providedPorts() 47 | { 48 | return { 49 | BT::InputPort("exception_code", 0, "Exception code for check."), 50 | BT::InputPort("expected_code", 0, "Expected code for check."), 51 | }; 52 | } 53 | 54 | private: 55 | /** 56 | * @brief The main override required by a BT action 57 | * @return BT::NodeStatus Status of tick execution 58 | */ 59 | BT::NodeStatus tick() override; 60 | rclcpp::Node::SharedPtr node_; 61 | }; 62 | 63 | } // namespace mcr_tracking_components 64 | 65 | #endif // mcr_tracking_components__PLUGINS__DECORATOR__SINGLE_TRIGGER_NODE_HPP_ 66 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/behavior_tree_nodes/follow_path_action.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 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 | #ifndef MCR_TRACKING_COMPONENTS__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_ 16 | #define MCR_TRACKING_COMPONENTS__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_ 17 | 18 | #include 19 | 20 | #include "nav2_msgs/action/follow_path.hpp" 21 | #include "nav2_behavior_tree/bt_action_node.hpp" 22 | 23 | namespace mcr_tracking_components 24 | { 25 | 26 | /** 27 | * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::FollowPath 28 | */ 29 | class FollowPathAction : public nav2_behavior_tree::BtActionNode 30 | { 31 | public: 32 | /** 33 | * @brief A constructor for nav2_behavior_tree::FollowPathAction 34 | * @param xml_tag_name Name for the XML tag for this node 35 | * @param action_name Action name this node creates a client for 36 | * @param conf BT node configuration 37 | */ 38 | FollowPathAction( 39 | const std::string & xml_tag_name, 40 | const std::string & action_name, 41 | const BT::NodeConfiguration & conf); 42 | 43 | /** 44 | * @brief Function to perform some user-defined operation on tick 45 | */ 46 | void on_tick() override; 47 | 48 | /** 49 | * @brief Function to perform some user-defined operation after a timeout 50 | * waiting for a result that hasn't been received yet 51 | */ 52 | void on_wait_for_result() override; 53 | BT::NodeStatus on_aborted() override; 54 | 55 | /** 56 | * @brief Creates list of BT ports 57 | * @return BT::PortsList Containing basic ports along with node-specific ports 58 | */ 59 | static BT::PortsList providedPorts() 60 | { 61 | return providedBasicPorts( 62 | { 63 | BT::InputPort("path", "Path to follow"), 64 | BT::InputPort("goal", "goal to reach"), 65 | BT::InputPort("controller_id", ""), 66 | BT::InputPort("goal_checker_id", ""), 67 | BT::InputPort("progress_checker_id", ""), 68 | BT::OutputPort("output_exception_code", ""), 69 | }); 70 | } 71 | }; 72 | 73 | } // namespace mcr_tracking_components 74 | 75 | #endif // MCR_TRACKING_COMPONENTS__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_ 76 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/behavior_tree_nodes/is_battery_in_charge_condition.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Sarthak Mittal 2 | // Copyright (c) 2019 Intel Corporation 3 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 4 | // 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | // 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | // 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | 17 | #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_IN_CHARGE_CONDITION_HPP_ 18 | #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_IN_CHARGE_CONDITION_HPP_ 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include "rclcpp/rclcpp.hpp" 25 | #include "sensor_msgs/msg/battery_state.hpp" 26 | #include "behaviortree_cpp_v3/condition_node.h" 27 | 28 | namespace mcr_tracking_components 29 | { 30 | 31 | /** 32 | * @brief A BT::ConditionNode that listens to a battery topic and 33 | * returns SUCCESS when battery is low and FAILURE otherwise 34 | */ 35 | class IsBatteryInChargeCondition : public BT::ConditionNode 36 | { 37 | public: 38 | /** 39 | * @brief A constructor for nav2_behavior_tree::IsBatteryInChargeCondition 40 | * @param condition_name Name for the XML tag for this node 41 | * @param conf BT node configuration 42 | */ 43 | IsBatteryInChargeCondition( 44 | const std::string & condition_name, 45 | const BT::NodeConfiguration & conf); 46 | 47 | IsBatteryInChargeCondition() = delete; 48 | 49 | /** 50 | * @brief The main override required by a BT action 51 | * @return BT::NodeStatus Status of tick execution 52 | */ 53 | BT::NodeStatus tick() override; 54 | 55 | /** 56 | * @brief Creates list of BT ports 57 | * @return BT::PortsList Containing node-specific ports 58 | */ 59 | static BT::PortsList providedPorts() 60 | { 61 | return { 62 | BT::InputPort("min_battery", "Minimum battery percentage/voltage"), 63 | BT::InputPort( 64 | "battery_topic", std::string("/battery_status"), "Battery topic"), 65 | BT::InputPort( 66 | "is_voltage", false, "If true voltage will be used to check for low battery"), 67 | }; 68 | } 69 | 70 | private: 71 | /** 72 | * @brief Callback function for battery topic 73 | * @param msg Shared pointer to sensor_msgs::msg::BatteryState message 74 | */ 75 | void batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg); 76 | 77 | rclcpp::Node::SharedPtr node_; 78 | rclcpp::CallbackGroup::SharedPtr callback_group_; 79 | rclcpp::executors::SingleThreadedExecutor callback_group_executor_; 80 | rclcpp::Subscription::SharedPtr battery_sub_; 81 | std::string battery_topic_; 82 | double min_battery_; 83 | bool is_voltage_; 84 | bool is_battery_low_; 85 | }; 86 | 87 | } // namespace mcr_tracking_components 88 | 89 | #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_CONDITION_HPP_ 90 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/behavior_tree_nodes/orientation_derivers.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef mcr_tracking_components__PLUGINS__ORIENTATION_DERIVERS_HPP_ 16 | #define mcr_tracking_components__PLUGINS__ORIENTATION_DERIVERS_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "geometry_msgs/msg/pose_stamped.hpp" 23 | #include 24 | #include "behaviortree_cpp_v3/decorator_node.h" 25 | #include 26 | #include 27 | #include "tf2_ros/transform_broadcaster.h" 28 | #include 29 | #include 30 | #include "tf2_ros/transform_listener.h" 31 | #include "tf2/utils.h" 32 | #include "tf2/LinearMath/Transform.h" 33 | #include "tf2_ros/buffer.h" 34 | #include "tf2_ros/buffer_interface.h" 35 | #include "tf2/buffer_core.h" 36 | #include "tf2_ros/message_filter.h" 37 | #include "tf2_ros/create_timer_ros.h" 38 | #include "rclcpp/rclcpp.hpp" 39 | #include "nav_msgs/msg/path.hpp" 40 | #include "Eigen/Eigen" 41 | #include "Eigen/Dense" 42 | #include "Eigen/QR" 43 | 44 | namespace mcr_tracking_components 45 | { 46 | class OrientationDeriver{ 47 | public: 48 | using Ptr = std::shared_ptr; 49 | virtual ~OrientationDeriver(){} 50 | virtual void initialize(const rclcpp::Node::SharedPtr node, 51 | const std::string &global_frame, 52 | const std::shared_ptr tf_buffer){ 53 | node_ = node; 54 | global_frame_ = global_frame; 55 | tf_buffer_ = tf_buffer; 56 | vx_ = 0.0, vy_ = 0.0; 57 | } 58 | virtual geometry_msgs::msg::PoseStamped deriveOrientation(const geometry_msgs::msg::PoseStamped::SharedPtr msg)=0; 59 | virtual double getVx(){return vx_;} 60 | virtual double getVy(){return vy_;} 61 | protected: 62 | rclcpp::Node::SharedPtr node_; 63 | double vx_, vy_; 64 | std::string global_frame_; 65 | std::shared_ptr tf_buffer_; 66 | std::deque historical_raw_poses_; 67 | }; 68 | 69 | 70 | class MeanOrientationDeriver : public OrientationDeriver{ 71 | public: 72 | void initialize(const rclcpp::Node::SharedPtr node, 73 | const std::string &global_frame, 74 | const std::shared_ptr tf_buffer) override; 75 | geometry_msgs::msg::PoseStamped deriveOrientation(const geometry_msgs::msg::PoseStamped::SharedPtr msg) override; 76 | }; 77 | 78 | #include "nav_msgs/msg/path.hpp" 79 | class FittingOrientationDeriver : public OrientationDeriver{ 80 | public: 81 | void initialize(const rclcpp::Node::SharedPtr node, 82 | const std::string &global_frame, 83 | const std::shared_ptr tf_buffer) override; 84 | geometry_msgs::msg::PoseStamped deriveOrientation(const geometry_msgs::msg::PoseStamped::SharedPtr msg) override; 85 | private: 86 | nav_msgs::msg::Path spline(const std::vector & poses); 87 | rclcpp::Publisher::SharedPtr plan_publisher_; 88 | 89 | }; 90 | 91 | class KalmanOrientationDeriver : public OrientationDeriver{ 92 | public: 93 | void initialize(const rclcpp::Node::SharedPtr node, 94 | const std::string &global_frame, 95 | const std::shared_ptr tf_buffer) override; 96 | geometry_msgs::msg::PoseStamped deriveOrientation(const geometry_msgs::msg::PoseStamped::SharedPtr msg) override; 97 | 98 | private: 99 | Eigen::Matrix x_, U_; // initial state, external motion. 100 | Eigen::Matrix4d P_, F_, I_; // initial uncertainty, next state function, identity matrix. 101 | Eigen::Matrix H_; // measurement function. 102 | Eigen::Matrix2d R_; // measurement uncertainty. 103 | 104 | }; 105 | 106 | } // namespace mcr_tracking_components 107 | 108 | #endif // mcr_tracking_components__PLUGINS__ORIENTATION_DERIVERS_HPP_ 109 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/behavior_tree_nodes/seat_adjust_client_node.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef mcr_tracking_components__PLUGINS__ACTION__SEAT_ADJUST_CLIENT_NODE_HPP_ 16 | #define mcr_tracking_components__PLUGINS__ACTION__SEAT_ADJUST_CLIENT_NODE_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | // #include "behaviortree_cpp_v3/action_node.h" 22 | #include "protocol/action/seat_adjust.hpp" 23 | #include "std_msgs/msg/int8.hpp" 24 | #include "nav2_behavior_tree/bt_action_node.hpp" 25 | 26 | 27 | namespace mcr_tracking_components { 28 | 29 | using SeatAdjustT = protocol::action::SeatAdjust; 30 | /** 31 | * @brief A BT::ActionNodeBase to make decision for tracking mode 32 | */ 33 | class SeatAdjustClient : public nav2_behavior_tree::BtActionNode { 34 | public: 35 | /** 36 | * @brief A constructor for mcr_tracking_components::SpinAndSearchAction 37 | * @param xml_tag_name Name for the XML tag for this node 38 | * @param action_name Action name this node creates a client for 39 | * @param conf BT node configuration 40 | */ 41 | SeatAdjustClient( 42 | const std::string & xml_tag_name, 43 | const std::string & action_name, 44 | const BT::NodeConfiguration & conf); 45 | 46 | /** 47 | * @brief Creates list of BT ports 48 | * @return BT::PortsList Containing basic ports along with node-specific ports 49 | */ 50 | static BT::PortsList providedPorts() 51 | { 52 | return providedBasicPorts( 53 | { 54 | BT::InputPort("start", "To start the action."), 55 | }); 56 | } 57 | 58 | virtual void on_wait_for_result() override; 59 | 60 | /** 61 | * @brief Function to perform some user-defined operation on tick 62 | */ 63 | void on_tick() override; 64 | }; 65 | 66 | } // namespace mcr_tracking_components 67 | 68 | #endif // mcr_tracking_components__PLUGINS__ACTION__CHANGE_GAIT_ACTION_HPP_ 69 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/behavior_tree_nodes/spin_and_search_action.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 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 | #ifndef mcr_tracking_components__PLUGINS__ACTION__SPIN_AND_SEARCH_ACTION_HPP_ 17 | #define mcr_tracking_components__PLUGINS__ACTION__SPIN_AND_SEARCH_ACTION_HPP_ 18 | 19 | #include 20 | 21 | #include "nav2_behavior_tree/bt_action_node.hpp" 22 | #include "nav2_msgs/action/spin.hpp" 23 | 24 | namespace mcr_tracking_components 25 | { 26 | 27 | /** 28 | * @brief A mcr_tracking_components::BtActionNode class that wraps nav2_msgs::action::Spin 29 | */ 30 | class SpinAndSearchAction : public nav2_behavior_tree::BtActionNode 31 | { 32 | public: 33 | /** 34 | * @brief A constructor for mcr_tracking_components::SpinAndSearchAction 35 | * @param xml_tag_name Name for the XML tag for this node 36 | * @param action_name Action name this node creates a client for 37 | * @param conf BT node configuration 38 | */ 39 | SpinAndSearchAction( 40 | const std::string & xml_tag_name, 41 | const std::string & action_name, 42 | const BT::NodeConfiguration & conf); 43 | 44 | /** 45 | * @brief Function to perform some user-defined operation on tick 46 | */ 47 | void on_tick() override; 48 | 49 | /** 50 | * @brief Creates list of BT ports 51 | * @return BT::PortsList Containing basic ports along with node-specific ports 52 | */ 53 | static BT::PortsList providedPorts() 54 | { 55 | return providedBasicPorts( 56 | { 57 | BT::InputPort("search_dist", 1.57, "Spin distance") 58 | }); 59 | } 60 | }; 61 | 62 | } // namespace mcr_tracking_components 63 | 64 | #endif // mcr_tracking_components__PLUGINS__ACTION__SPIN_ACTION_HPP_ 65 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/behavior_tree_nodes/tracking_mode_decider_node.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef mcr_tracking_components__PLUGINS__ACTION__TRACKING_MODE_DECIDER_NODE_HPP_ 16 | #define mcr_tracking_components__PLUGINS__ACTION__TRACKING_MODE_DECIDER_NODE_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "nav_msgs/msg/path.hpp" 22 | 23 | #include "behaviortree_cpp_v3/action_node.h" 24 | #include "mcr_msgs/srv/switch_mode.hpp" 25 | namespace mcr_tracking_components 26 | { 27 | 28 | /** 29 | * @brief A BT::ActionNodeBase to make decision for tracking mode 30 | */ 31 | class TrackingModeDecider : public BT::ActionNodeBase 32 | { 33 | public: 34 | /** 35 | * @brief A mcr_tracking_components::TrackingModeDecider constructor 36 | * @param xml_tag_name Name for the XML tag for this node 37 | * @param conf BT node configuration 38 | */ 39 | TrackingModeDecider( 40 | const std::string & xml_tag_name, 41 | const BT::NodeConfiguration & conf); 42 | 43 | /** 44 | * @brief Creates list of BT ports 45 | * @return BT::PortsList Containing basic ports along with node-specific ports 46 | */ 47 | static BT::PortsList providedPorts() 48 | { 49 | return { 50 | BT::InputPort("input_tracking_mode", "Original tracking mode"), 51 | BT::OutputPort( 52 | "output_tracking_mode", 53 | "Output tracking mode by decision"), 54 | }; 55 | } 56 | 57 | private: 58 | /** 59 | * @brief The other (optional) override required by a BT action. 60 | */ 61 | void halt() override {} 62 | void modeSwitchCallback( 63 | const std::shared_ptr request, 64 | std::shared_ptr response); 65 | /** 66 | * @brief The main override required by a BT action 67 | * @return BT::NodeStatus Status of tick execution 68 | */ 69 | rclcpp::Node::SharedPtr node_; 70 | rclcpp::CallbackGroup::SharedPtr callback_group_; 71 | rclcpp::executors::SingleThreadedExecutor callback_group_executor_; 72 | BT::NodeStatus tick() override; 73 | unsigned char last_mode_ = 255; 74 | rclcpp::Service::SharedPtr service_; 75 | 76 | }; 77 | 78 | } // namespace mcr_tracking_components 79 | 80 | #endif // mcr_tracking_components__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_ 81 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/controller_plugins/fake_progress_checker.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 mcr_tracking_components__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_ 15 | #define mcr_tracking_components__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_ 16 | 17 | #include 18 | #include "rclcpp/rclcpp.hpp" 19 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 20 | #include "nav2_core/progress_checker.hpp" 21 | #include "geometry_msgs/msg/pose_stamped.hpp" 22 | #include "geometry_msgs/msg/pose2_d.hpp" 23 | 24 | namespace mcr_tracking_components 25 | { 26 | /** 27 | * @class FakeProgressChecker 28 | * @brief This plugin is used to check the position of the robot to make sure 29 | * that it is actually progressing towards a goal. 30 | */ 31 | 32 | class FakeProgressChecker : public nav2_core::ProgressChecker 33 | { 34 | public: 35 | void initialize( 36 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, 37 | const std::string & plugin_name) override; 38 | bool check(geometry_msgs::msg::PoseStamped & current_pose) override; 39 | void reset() override; 40 | 41 | protected: 42 | /** 43 | * @brief Calculates robots movement from baseline pose 44 | * @param pose Current pose of the robot 45 | * @return true, if movement is greater than radius_, or false 46 | */ 47 | bool is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose); 48 | /** 49 | * @brief Resets baseline pose with the current pose of the robot 50 | * @param pose Current pose of the robot 51 | */ 52 | void reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose); 53 | 54 | rclcpp::Clock::SharedPtr clock_; 55 | 56 | double radius_; 57 | rclcpp::Duration time_allowance_{0, 0}; 58 | 59 | geometry_msgs::msg::Pose2D baseline_pose_; 60 | rclcpp::Time baseline_time_; 61 | 62 | bool baseline_pose_set_{false}; 63 | // Subscription for parameter change 64 | rclcpp::AsyncParametersClient::SharedPtr parameters_client_; 65 | rclcpp::Subscription::SharedPtr parameter_event_sub_; 66 | std::string plugin_name_; 67 | 68 | /** 69 | * @brief Callback executed when a paramter change is detected 70 | * @param event ParameterEvent message 71 | */ 72 | void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); 73 | }; 74 | } // namespace mcr_tracking_components 75 | 76 | #endif // mcr_tracking_components__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_ 77 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/controller_plugins/unreachable_goal_checker.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef mcr_tracking_components__PLUGINS__UNREACHABLE_GOAL_CHECKER_HPP_ 16 | #define mcr_tracking_components__PLUGINS__UNREACHABLE_GOAL_CHECKER_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "rclcpp/rclcpp.hpp" 22 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 23 | #include "nav2_core/goal_checker.hpp" 24 | 25 | namespace mcr_tracking_components 26 | { 27 | 28 | /** 29 | * @class UnreachableGoalChecker 30 | * @brief Goal Checker plugin that only checks the position difference 31 | * 32 | * This class can be stateful if the stateful parameter is set to true (which it is by default). 33 | * This means that the goal checker will not check if the xy position matches again once it is found to be true. 34 | */ 35 | class UnreachableGoalChecker : public nav2_core::GoalChecker 36 | { 37 | public: 38 | UnreachableGoalChecker(); 39 | // Standard GoalChecker Interface 40 | void initialize( 41 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, 42 | const std::string & plugin_name) override; 43 | void reset() override; 44 | bool isGoalReached( 45 | const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, 46 | const geometry_msgs::msg::Twist & velocity) override; 47 | bool getTolerances( 48 | geometry_msgs::msg::Pose & pose_tolerance, 49 | geometry_msgs::msg::Twist & vel_tolerance) override; 50 | 51 | protected: 52 | double xy_goal_tolerance_, yaw_goal_tolerance_; 53 | bool stateful_, check_xy_; 54 | // Cached squared xy_goal_tolerance_ 55 | double xy_goal_tolerance_sq_; 56 | // Subscription for parameter change 57 | rclcpp::AsyncParametersClient::SharedPtr parameters_client_; 58 | rclcpp::Subscription::SharedPtr parameter_event_sub_; 59 | std::string plugin_name_; 60 | 61 | /** 62 | * @brief Callback executed when a paramter change is detected 63 | * @param event ParameterEvent message 64 | */ 65 | void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); 66 | }; 67 | 68 | } // namespace mcr_tracking_components 69 | 70 | #endif // mcr_tracking_components__PLUGINS__UNREACHABLE_GOAL_CHECKER_HPP_ 71 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/controller_plugins/x_goal_checker.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef mcr_tracking_components__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ 16 | #define mcr_tracking_components__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "rclcpp/rclcpp.hpp" 22 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 23 | #include "nav2_core/goal_checker.hpp" 24 | #include "geometry_msgs/msg/pose_stamped.hpp" 25 | 26 | namespace mcr_tracking_components 27 | { 28 | 29 | /** 30 | * @class SimpleGoalChecker 31 | * @brief Goal Checker plugin that only checks the position difference 32 | * 33 | * This class can be stateful if the stateful parameter is set to true (which it is by default). 34 | * This means that the goal checker will not check if the xy position matches again once it is found to be true. 35 | */ 36 | class XGoalChecker : public nav2_core::GoalChecker 37 | { 38 | public: 39 | XGoalChecker(); 40 | // Standard GoalChecker Interface 41 | void initialize( 42 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, 43 | const std::string & plugin_name) override; 44 | void reset() override; 45 | bool isGoalReached( 46 | const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, 47 | const geometry_msgs::msg::Twist & velocity) override; 48 | bool getTolerances( 49 | geometry_msgs::msg::Pose & pose_tolerance, 50 | geometry_msgs::msg::Twist & vel_tolerance) override; 51 | void callback_updated_goal( 52 | const geometry_msgs::msg::PoseStamped::SharedPtr msg); 53 | protected: 54 | double xy_goal_tolerance_, yaw_goal_tolerance_; 55 | double current_pose_x_; 56 | bool stateful_, check_xy_; 57 | // Cached squared xy_goal_tolerance_ 58 | double xy_goal_tolerance_sq_; 59 | // Subscription for parameter change 60 | rclcpp::AsyncParametersClient::SharedPtr parameters_client_; 61 | rclcpp::Subscription::SharedPtr parameter_event_sub_; 62 | rclcpp::Subscription::SharedPtr goal_sub_; 63 | std::string plugin_name_; 64 | std::string goal_updater_topic; 65 | /** 66 | * @brief Callback executed when a paramter change is detected 67 | * @param event ParameterEvent message 68 | */ 69 | void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); 70 | }; 71 | 72 | } // namespace nav2_controller 73 | 74 | #endif // NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ 75 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/controller_plugins/xy_goal_checker.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef mcr_tracking_components__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ 16 | #define mcr_tracking_components__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "rclcpp/rclcpp.hpp" 22 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 23 | #include "nav2_core/goal_checker.hpp" 24 | 25 | namespace mcr_tracking_components 26 | { 27 | 28 | /** 29 | * @class XYGoalChecker 30 | * @brief Goal Checker plugin that only checks the position difference 31 | * 32 | * This class can be stateful if the stateful parameter is set to true (which it is by default). 33 | * This means that the goal checker will not check if the xy position matches again once it is found to be true. 34 | */ 35 | class XYGoalChecker : public nav2_core::GoalChecker 36 | { 37 | public: 38 | XYGoalChecker(); 39 | // Standard GoalChecker Interface 40 | void initialize( 41 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, 42 | const std::string & plugin_name) override; 43 | void reset() override; 44 | bool isGoalReached( 45 | const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, 46 | const geometry_msgs::msg::Twist & velocity) override; 47 | bool getTolerances( 48 | geometry_msgs::msg::Pose & pose_tolerance, 49 | geometry_msgs::msg::Twist & vel_tolerance) override; 50 | 51 | protected: 52 | double xy_goal_tolerance_, yaw_goal_tolerance_; 53 | bool stateful_, check_xy_; 54 | // Cached squared xy_goal_tolerance_ 55 | double xy_goal_tolerance_sq_; 56 | // Subscription for parameter change 57 | rclcpp::AsyncParametersClient::SharedPtr parameters_client_; 58 | rclcpp::Subscription::SharedPtr parameter_event_sub_; 59 | std::string plugin_name_; 60 | 61 | /** 62 | * @brief Callback executed when a paramter change is detected 63 | * @param event ParameterEvent message 64 | */ 65 | void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); 66 | }; 67 | 68 | } // namespace mcr_tracking_components 69 | 70 | #endif // mcr_tracking_components__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ 71 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/dwb_critics/keep_person_insight.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef mcr_tracking_components__KEEP_PERSON_INSIGHT_HPP_ 16 | #define mcr_tracking_components__KEEP_PERSON_INSIGHT_HPP_ 17 | 18 | #include 19 | #include 20 | #include "dwb_core/trajectory_critic.hpp" 21 | #include "geometry_msgs/msg/pose_stamped.hpp" 22 | #include "protocol/msg/person.hpp" 23 | #include "std_srvs/srv/set_bool.hpp" 24 | 25 | namespace mcr_tracking_components 26 | { 27 | 28 | 29 | class KeepPersonInsightCritic : public dwb_core::TrajectoryCritic 30 | { 31 | public: 32 | void onInit() override; 33 | void reset() override; 34 | bool prepare( 35 | const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel, 36 | const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan) override; 37 | double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override; 38 | 39 | virtual double scoreRotation(const dwb_msgs::msg::Trajectory2D & traj); 40 | 41 | private: 42 | void poseCallback(protocol::msg::Person::SharedPtr msg); 43 | // rclcpp::Node::SharedPtr rcl_node_; 44 | std::shared_ptr rcl_node_; 45 | rclcpp::Subscription::SharedPtr pose_sub_; 46 | protocol::msg::Person latest_person_; 47 | std::shared_ptr tf_buffer_; 48 | std::shared_ptr tf_listener_; 49 | double lookahead_time_; 50 | rclcpp::Time latest_timestamp_; 51 | double normal_sacle_; 52 | double goal_yaw_, cur_yaw_; 53 | bool inuse_; 54 | std::string target_topic_; 55 | }; 56 | 57 | } // namespace mcr_tracking_components 58 | #endif // mcr_tracking_components__KEEP_PERSON_INSIGHT_HPP_ 59 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/dwb_critics/keep_target_insight.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef mcr_tracking_components__KEEP_TARGET_INSIGHT_HPP_ 16 | #define mcr_tracking_components__KEEP_TARGET_INSIGHT_HPP_ 17 | 18 | #include 19 | #include 20 | #include "dwb_core/trajectory_critic.hpp" 21 | #include "geometry_msgs/msg/pose_stamped.hpp" 22 | #include "std_srvs/srv/set_bool.hpp" 23 | 24 | namespace mcr_tracking_components 25 | { 26 | 27 | 28 | class KeepTargetInsightCritic : public dwb_core::TrajectoryCritic 29 | { 30 | public: 31 | void onInit() override; 32 | void reset() override; 33 | bool prepare( 34 | const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel, 35 | const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan) override; 36 | double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override; 37 | 38 | virtual double scoreRotation(const dwb_msgs::msg::Trajectory2D & traj); 39 | 40 | private: 41 | void poseCallback(geometry_msgs::msg::PoseStamped::SharedPtr msg); 42 | void useCriticCallback( 43 | const std::shared_ptr request, 44 | std::shared_ptr response); 45 | rclcpp::Subscription::SharedPtr pose_sub_; 46 | rclcpp::Service::SharedPtr service_; 47 | geometry_msgs::msg::PoseStamped latest_pose_; 48 | std::shared_ptr tf_buffer_; 49 | std::shared_ptr tf_listener_; 50 | double lookahead_time_; 51 | double normal_sacle_; 52 | double goal_yaw_, cur_yaw_; 53 | bool valid_data_, inuse_; 54 | std::string target_topic_; 55 | }; 56 | 57 | } // namespace mcr_tracking_components 58 | #endif // mcr_tracking_components__ROTATE_TO_GOAL_HPP_ 59 | -------------------------------------------------------------------------------- /mcr_tracking_components/include/mcr_tracking_components/recoveries/spin_and_search.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 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 | #ifndef mcr_tracking_components__PLUGINS__SPIN_AND_SEARCH_HPP_ 17 | #define mcr_tracking_components__PLUGINS__SPIN_AND_SEARCH_HPP_ 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "nav2_recoveries/recovery.hpp" 24 | #include "nav2_msgs/action/spin.hpp" 25 | #include "geometry_msgs/msg/quaternion.hpp" 26 | #include "nav2_util/lifecycle_node.hpp" 27 | #include "geometry_msgs/msg/pose_stamped.hpp" 28 | 29 | namespace mcr_tracking_components 30 | { 31 | using SpinAction = nav2_msgs::action::Spin; 32 | 33 | /** 34 | * @class mcr_tracking_components::Spin 35 | * @brief An action server recovery for spinning in 36 | */ 37 | class SpinAndSearch : public nav2_recoveries::Recovery 38 | { 39 | public: 40 | /** 41 | * @brief A constructor for mcr_tracking_components::SpinAndSearch 42 | */ 43 | SpinAndSearch(); 44 | ~SpinAndSearch(); 45 | 46 | /** 47 | * @brief Initialization to run behavior 48 | * @param command Goal to execute 49 | * @return Status of recovery 50 | */ 51 | nav2_recoveries::Status onRun(const std::shared_ptr command) override; 52 | 53 | /** 54 | * @brief Configuration of recovery action 55 | */ 56 | void onConfigure() override; 57 | 58 | /** 59 | * @brief Loop function to run behavior 60 | * @return Status of recovery 61 | */ 62 | nav2_recoveries::Status onCycleUpdate() override; 63 | void callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg); 64 | 65 | protected: 66 | /** 67 | * @brief Check if pose is collision free 68 | * @param distance Distance to check forward 69 | * @param cmd_vel current commanded velocity 70 | * @param pose2d Current pose 71 | * @return is collision free or not 72 | */ 73 | bool isCollisionFree( 74 | const double & distance, 75 | geometry_msgs::msg::Twist * cmd_vel, 76 | geometry_msgs::msg::Pose2D & pose2d); 77 | 78 | rclcpp_lifecycle::LifecycleNode::SharedPtr shared_node_; 79 | SpinAction::Feedback::SharedPtr feedback_; 80 | rclcpp::Subscription::SharedPtr goal_sub_; 81 | std::string goal_updater_topic_; 82 | rclcpp::Time latest_timestamped_; 83 | geometry_msgs::msg::PoseStamped latest_posestamped_; 84 | 85 | double min_rotational_vel_; 86 | double max_rotational_vel_; 87 | double rotational_acc_lim_; 88 | double cmd_yaw_; 89 | double prev_yaw_; 90 | double relative_yaw_; 91 | double simulate_ahead_time_; 92 | }; 93 | 94 | } // namespace mcr_tracking_components 95 | 96 | #endif // NAV2_RECOVERIES__PLUGINS__SPIN_HPP_ 97 | -------------------------------------------------------------------------------- /mcr_tracking_components/orientation_derivers.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mean value for orientation from the target's historical poses. 5 | 6 | 7 | least-square polynomial fitting for orientation from the target's historical poses. 8 | 9 | 10 | kalman filter for orientation from the target's historical poses. 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /mcr_tracking_components/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mcr_tracking_components 5 | 1.0.8 6 | TODO 7 | Michael Jeronimo 8 | Carlos Orduno 9 | Carlos Orduno 10 | Apache-2.0 11 | 12 | ament_cmake 13 | 14 | tf2_geometry_msgs 15 | std_srvs 16 | 17 | rclcpp 18 | rclcpp_action 19 | rclcpp_lifecycle 20 | behaviortree_cpp_v3 21 | builtin_interfaces 22 | geometry_msgs 23 | sensor_msgs 24 | nav2_msgs 25 | mcr_msgs 26 | nav2_behavior_tree 27 | nav2_core 28 | dwb_core 29 | nav2_costmap_2d 30 | nav2_recoveries 31 | nav_msgs 32 | tf2 33 | tf2_ros 34 | tf2_geometry_msgs 35 | std_msgs 36 | std_srvs 37 | nav2_util 38 | lifecycle_msgs 39 | nav2_common 40 | 41 | rclcpp 42 | rclcpp_action 43 | rclcpp_lifecycle 44 | std_msgs 45 | behaviortree_cpp_v3 46 | builtin_interfaces 47 | geometry_msgs 48 | sensor_msgs 49 | nav2_msgs 50 | mcr_msgs 51 | nav2_behavior_tree 52 | nav2_core 53 | nav2_costmap_2d 54 | dwb_core 55 | nav2_recoveries 56 | nav_msgs 57 | tf2 58 | tf2_ros 59 | tf2_geometry_msgs 60 | nav2_util 61 | lifecycle_msgs 62 | 63 | ament_lint_common 64 | ament_lint_auto 65 | ament_cmake_gtest 66 | 67 | 68 | ament_cmake 69 | 70 | 71 | -------------------------------------------------------------------------------- /mcr_tracking_components/recoveries.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /mcr_tracking_components/src/behavior_tree_nodes/compute_path_spline_poses_action.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Samsung Research America 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 | #include 17 | #include 18 | #include 19 | 20 | #include "mcr_tracking_components/behavior_tree_nodes/compute_path_spline_poses_action.hpp" 21 | 22 | namespace mcr_tracking_components 23 | { 24 | 25 | ComputePathSplinePosesAction::ComputePathSplinePosesAction( 26 | const std::string & xml_tag_name, 27 | const std::string & action_name, 28 | const BT::NodeConfiguration & conf) 29 | : BtActionNode(xml_tag_name, action_name, conf) 30 | { 31 | } 32 | 33 | void ComputePathSplinePosesAction::on_tick() 34 | { 35 | getInput("poses", goal_.poses); 36 | getInput("planner_id", goal_.planner_id); 37 | } 38 | 39 | BT::NodeStatus ComputePathSplinePosesAction::on_success() 40 | { 41 | setOutput("path", result_.result->path); 42 | return BT::NodeStatus::SUCCESS; 43 | } 44 | 45 | } // namespace mcr_tracking_components 46 | 47 | #include "behaviortree_cpp_v3/bt_factory.h" 48 | BT_REGISTER_NODES(factory) 49 | { 50 | BT::NodeBuilder builder = 51 | [](const std::string & name, const BT::NodeConfiguration & config) 52 | { 53 | return std::make_unique( 54 | name, "compute_path_spline_poses", config); 55 | }; 56 | 57 | factory.registerBuilder( 58 | "ComputePathSplinePoses", builder); 59 | } 60 | -------------------------------------------------------------------------------- /mcr_tracking_components/src/behavior_tree_nodes/compute_path_to_pose_action.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 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 | #include 17 | #include 18 | 19 | #include "mcr_tracking_components/behavior_tree_nodes/compute_path_to_pose_action.hpp" 20 | 21 | namespace mcr_tracking_components 22 | { 23 | 24 | ComputePathToPoseAction::ComputePathToPoseAction( 25 | const std::string & xml_tag_name, 26 | const std::string & action_name, 27 | const BT::NodeConfiguration & conf) 28 | : BtActionNode(xml_tag_name, action_name, conf) 29 | { 30 | } 31 | 32 | void ComputePathToPoseAction::on_tick() 33 | { 34 | getInput("goal", goal_.goal); 35 | getInput("planner_id", goal_.planner_id); 36 | if (getInput("start", goal_.start)) { 37 | goal_.use_start = true; 38 | } 39 | goal_updated_ = true; 40 | } 41 | 42 | void ComputePathToPoseAction::on_wait_for_result() 43 | { 44 | // geometry_msgs::msg::PoseStamped goal; 45 | // getInput("goal", goal); 46 | // if (goal_.goal != goal) { 47 | // goal_.goal = goal; 48 | // goal_updated_ = true; 49 | // } 50 | } 51 | 52 | BT::NodeStatus ComputePathToPoseAction::on_success() 53 | { 54 | setOutput("path", result_.result->path); 55 | return BT::NodeStatus::SUCCESS; 56 | } 57 | 58 | } // namespace mcr_tracking_components 59 | 60 | #include "behaviortree_cpp_v3/bt_factory.h" 61 | BT_REGISTER_NODES(factory) 62 | { 63 | BT::NodeBuilder builder = 64 | [](const std::string & name, const BT::NodeConfiguration & config) 65 | { 66 | return std::make_unique( 67 | name, "compute_path_to_p", config); 68 | }; 69 | 70 | factory.registerBuilder( 71 | "ComputePathToP", builder); 72 | } 73 | -------------------------------------------------------------------------------- /mcr_tracking_components/src/behavior_tree_nodes/exception_verify_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "mcr_tracking_components/behavior_tree_nodes/exception_verify_node.hpp" 19 | 20 | namespace mcr_tracking_components 21 | { 22 | 23 | ExceptionVerify::ExceptionVerify( 24 | const std::string & name, 25 | const BT::NodeConfiguration & conf) 26 | : BT::DecoratorNode(name, conf) 27 | { 28 | node_ = config().blackboard->get("node"); 29 | } 30 | 31 | BT::NodeStatus ExceptionVerify::tick() 32 | { 33 | setStatus(BT::NodeStatus::RUNNING); 34 | unsigned int a, b; 35 | getInput("exception_code", a); 36 | getInput("expected_code", b); 37 | if (a != b) { 38 | return BT::NodeStatus::FAILURE; 39 | } 40 | return child_node_->executeTick(); 41 | } 42 | 43 | } // namespace mcr_tracking_components 44 | 45 | #include "behaviortree_cpp_v3/bt_factory.h" 46 | BT_REGISTER_NODES(factory) 47 | { 48 | factory.registerNodeType("ExceptionVerify"); 49 | } 50 | -------------------------------------------------------------------------------- /mcr_tracking_components/src/behavior_tree_nodes/follow_path_action.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 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 | #include 17 | #include 18 | 19 | #include "mcr_tracking_components/behavior_tree_nodes/follow_path_action.hpp" 20 | #include "nav2_core/exceptions.hpp" 21 | namespace mcr_tracking_components 22 | { 23 | 24 | FollowPathAction::FollowPathAction( 25 | const std::string & xml_tag_name, 26 | const std::string & action_name, 27 | const BT::NodeConfiguration & conf) 28 | : BtActionNode(xml_tag_name, action_name, conf) 29 | { 30 | } 31 | 32 | void FollowPathAction::on_tick() 33 | { 34 | getInput("path", goal_.path); 35 | getInput("goal", goal_.goal); 36 | getInput("controller_id", goal_.controller_id); 37 | getInput("goal_checker_id", goal_.goal_checker_id); 38 | getInput("progress_checker_id", goal_.progress_checker_id); 39 | } 40 | 41 | void FollowPathAction::on_wait_for_result() 42 | { 43 | // Grab the new path 44 | nav_msgs::msg::Path new_path; 45 | int exception_code = 0; 46 | getInput("path", new_path); 47 | config().blackboard->get("exception_code", exception_code); 48 | 49 | // Check if it is not same with the current one 50 | if (goal_.path != new_path) { 51 | // the action server on the next loop iteration 52 | goal_.path = new_path; 53 | if(exception_code == nav2_core::CONTROLLEREXECPTION){ 54 | config().blackboard->set("exception_code", nav2_core::NOEXCEPTION); 55 | } 56 | goal_updated_ = true; 57 | } 58 | geometry_msgs::msg::PoseStamped new_goal; 59 | getInput("goal", new_goal); 60 | 61 | // Check if it is not same with the current one 62 | if (goal_.goal != new_goal) { 63 | // the action server on the next loop iteration 64 | goal_.goal = new_goal; 65 | goal_updated_ = true; 66 | } 67 | 68 | std::string new_controller_id; 69 | getInput("controller_id", new_controller_id); 70 | 71 | if (goal_.controller_id != new_controller_id) { 72 | goal_.controller_id = new_controller_id; 73 | goal_updated_ = true; 74 | } 75 | 76 | std::string new_goal_checker_id; 77 | getInput("goal_checker_id", new_goal_checker_id); 78 | 79 | if (goal_.goal_checker_id != new_goal_checker_id) { 80 | goal_.goal_checker_id = new_goal_checker_id; 81 | goal_updated_ = true; 82 | } 83 | std::string new_progress_checker_id; 84 | getInput("progress_checker_id", new_progress_checker_id); 85 | if (goal_.progress_checker_id != new_progress_checker_id) { 86 | goal_.progress_checker_id = new_progress_checker_id; 87 | goal_updated_ = true; 88 | } 89 | } 90 | 91 | BT::NodeStatus FollowPathAction::on_aborted() 92 | { 93 | config().blackboard->set("exception_code", nav2_core::CONTROLLEREXECPTION); 94 | setOutput("output_exception_code", nav2_core::CONTROLLEREXECPTION); 95 | return BT::NodeStatus::FAILURE; 96 | } 97 | 98 | } // namespace mcr_tracking_components 99 | 100 | #include "behaviortree_cpp_v3/bt_factory.h" 101 | BT_REGISTER_NODES(factory) 102 | { 103 | BT::NodeBuilder builder = 104 | [](const std::string & name, const BT::NodeConfiguration & config) 105 | { 106 | return std::make_unique( 107 | name, "follow_p", config); 108 | }; 109 | 110 | factory.registerBuilder( 111 | "FollowP", builder); 112 | } 113 | -------------------------------------------------------------------------------- /mcr_tracking_components/src/behavior_tree_nodes/is_battery_in_charge_condition.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Sarthak Mittal 2 | // Copyright (c) 2019 Intel Corporation 3 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 4 | // 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | // 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | // 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | 17 | #include "mcr_tracking_components/behavior_tree_nodes/is_battery_in_charge_condition.hpp" 18 | 19 | #include 20 | 21 | namespace mcr_tracking_components { 22 | 23 | IsBatteryInChargeCondition::IsBatteryInChargeCondition( 24 | const std::string& condition_name, const BT::NodeConfiguration& conf) 25 | : BT::ConditionNode(condition_name, conf), 26 | battery_topic_("/battery_status"), 27 | min_battery_(0.0), 28 | is_voltage_(false), 29 | is_battery_low_(false) { 30 | getInput("min_battery", min_battery_); 31 | getInput("battery_topic", battery_topic_); 32 | getInput("is_voltage", is_voltage_); 33 | node_ = config().blackboard->get("node"); 34 | callback_group_ = node_->create_callback_group( 35 | rclcpp::CallbackGroupType::MutuallyExclusive, false); 36 | callback_group_executor_.add_callback_group(callback_group_, 37 | node_->get_node_base_interface()); 38 | 39 | rclcpp::SubscriptionOptions sub_option; 40 | sub_option.callback_group = callback_group_; 41 | battery_sub_ = node_->create_subscription( 42 | battery_topic_, rclcpp::SystemDefaultsQoS(), 43 | std::bind(&IsBatteryInChargeCondition::batteryCallback, this, 44 | std::placeholders::_1), 45 | sub_option); 46 | } 47 | 48 | BT::NodeStatus IsBatteryInChargeCondition::tick() { 49 | callback_group_executor_.spin_some(); 50 | // if (is_battery_low_) { 51 | // return BT::NodeStatus::SUCCESS; 52 | // } 53 | // return BT::NodeStatus::FAILURE; 54 | RCLCPP_INFO(node_->get_logger(), "Recharge successful."); 55 | return BT::NodeStatus::SUCCESS; 56 | } 57 | 58 | void IsBatteryInChargeCondition::batteryCallback( 59 | sensor_msgs::msg::BatteryState::SharedPtr msg) { 60 | if (is_voltage_) { 61 | is_battery_low_ = msg->voltage <= min_battery_; 62 | } else { 63 | is_battery_low_ = msg->percentage <= min_battery_; 64 | } 65 | } 66 | 67 | } // namespace mcr_tracking_components 68 | 69 | #include "behaviortree_cpp_v3/bt_factory.h" 70 | BT_REGISTER_NODES(factory) { 71 | factory.registerNodeType( 72 | "IsBatteryInCharge"); 73 | } 74 | -------------------------------------------------------------------------------- /mcr_tracking_components/src/behavior_tree_nodes/seat_adjust_client_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "mcr_tracking_components/behavior_tree_nodes/seat_adjust_client_node.hpp" 19 | 20 | namespace mcr_tracking_components { 21 | 22 | SeatAdjustClient::SeatAdjustClient( 23 | const std::string & xml_tag_name, 24 | const std::string & action_name, 25 | const BT::NodeConfiguration & conf) 26 | : BtActionNode(xml_tag_name, action_name, conf) 27 | { 28 | } 29 | 30 | void SeatAdjustClient::on_wait_for_result() 31 | { 32 | if (status() == BT::NodeStatus::SUCCESS) { 33 | // this->goal_done_ = true; 34 | RCLCPP_INFO(node_->get_logger(), "--------------on_wait_for_result SUCCESS --SeatAdjustClient"); 35 | } 36 | RCLCPP_INFO(node_->get_logger(), "--------------on_wait_for_result --SeatAdjustClient"); 37 | } 38 | void SeatAdjustClient::on_tick() 39 | { 40 | if (!getInput("start", goal_.start)) { 41 | RCLCPP_ERROR( 42 | node_->get_logger(), 43 | "-----------------------------SeatAdjustClient: goal not provided"); 44 | return; 45 | } 46 | RCLCPP_INFO(node_->get_logger(), "------------------------------on_tick--SeatAdjustClient"); 47 | } 48 | } 49 | #include "behaviortree_cpp_v3/bt_factory.h" 50 | BT_REGISTER_NODES(factory) 51 | { 52 | BT::NodeBuilder builder = 53 | [](const std::string & name, const BT::NodeConfiguration & config) 54 | { 55 | return std::make_unique( 56 | name, "seatadjust", config); 57 | }; 58 | 59 | factory.registerBuilder( 60 | "SeatAdjustClient", builder); 61 | } -------------------------------------------------------------------------------- /mcr_tracking_components/src/behavior_tree_nodes/spin_and_search_action.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 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 | #include 17 | #include 18 | 19 | #include "mcr_tracking_components/behavior_tree_nodes/spin_and_search_action.hpp" 20 | 21 | namespace mcr_tracking_components 22 | { 23 | 24 | SpinAndSearchAction::SpinAndSearchAction( 25 | const std::string & xml_tag_name, 26 | const std::string & action_name, 27 | const BT::NodeConfiguration & conf) 28 | : BtActionNode(xml_tag_name, action_name, conf) 29 | { 30 | double dist; 31 | getInput("search_dist", dist); 32 | goal_.target_yaw = dist; 33 | } 34 | 35 | void SpinAndSearchAction::on_tick() 36 | { 37 | increment_recovery_count(); 38 | } 39 | 40 | } // namespace mcr_tracking_components 41 | 42 | #include "behaviortree_cpp_v3/bt_factory.h" 43 | BT_REGISTER_NODES(factory) 44 | { 45 | BT::NodeBuilder builder = 46 | [](const std::string & name, const BT::NodeConfiguration & config) 47 | { 48 | return std::make_unique( 49 | name, "spin_and_search", 50 | config); 51 | }; 52 | 53 | factory.registerBuilder("SpinAndSearch", builder); 54 | } 55 | -------------------------------------------------------------------------------- /mcr_tracking_components/src/behavior_tree_nodes/tracking_mode_decider_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "rclcpp/rclcpp.hpp" 20 | #include "nav_msgs/msg/path.hpp" 21 | #include "geometry_msgs/msg/pose_stamped.hpp" 22 | #include "nav2_util/geometry_utils.hpp" 23 | #include "behaviortree_cpp_v3/decorator_node.h" 24 | #include "mcr_msgs/action/target_tracking.hpp" 25 | #include "mcr_tracking_components/behavior_tree_nodes/tracking_mode_decider_node.hpp" 26 | 27 | namespace mcr_tracking_components 28 | { 29 | 30 | TrackingModeDecider::TrackingModeDecider( 31 | const std::string & name, 32 | const BT::NodeConfiguration & conf) 33 | : BT::ActionNodeBase(name, conf) 34 | { 35 | node_ = config().blackboard->get("node"); 36 | callback_group_ = node_->create_callback_group( 37 | rclcpp::CallbackGroupType::MutuallyExclusive, 38 | false); 39 | callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); 40 | 41 | service_ = node_->create_service( 42 | "switch_mode", 43 | std::bind( 44 | &TrackingModeDecider::modeSwitchCallback, this, std::placeholders::_1, 45 | std::placeholders::_2), 46 | rmw_qos_profile_default, 47 | callback_group_); 48 | 49 | // getInput("distance", distance_); 50 | } 51 | std::string dir_analysis(unsigned char dir) 52 | { 53 | std::vector dirs{"", "Behind", "Left", "Right", "Unkonwn"}; 54 | if(dir > 3)dir = 4; 55 | return dirs[dir]; 56 | } 57 | 58 | inline BT::NodeStatus TrackingModeDecider::tick() 59 | { 60 | callback_group_executor_.spin_some(); 61 | setStatus(BT::NodeStatus::RUNNING); 62 | unsigned char current_mode; 63 | 64 | getInput("input_tracking_mode", current_mode); 65 | if(current_mode > 3){ 66 | current_mode = 1; 67 | } 68 | RCLCPP_INFO(node_->get_logger(), 69 | "Next, use the %s tracking mode", dir_analysis( 70 | current_mode).c_str()); 71 | setOutput("output_tracking_mode", current_mode); 72 | return BT::NodeStatus::SUCCESS; 73 | } 74 | 75 | void TrackingModeDecider::modeSwitchCallback( 76 | const std::shared_ptr request, 77 | std::shared_ptr response) 78 | { 79 | config().blackboard->set("relative_pos", request->relative_pos); 80 | std::cout << request->relative_pos << std::endl; 81 | response->result = true; 82 | } 83 | 84 | } // namespace mcr_tracking_components 85 | 86 | #include "behaviortree_cpp_v3/bt_factory.h" 87 | BT_REGISTER_NODES(factory) 88 | { 89 | factory.registerNodeType("TrackingModeDecider"); 90 | } 91 | -------------------------------------------------------------------------------- /mcr_tracking_components/src/controller_plugins/fake_progress_checker.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 "mcr_tracking_components/controller_plugins/fake_progress_checker.hpp" 16 | #include 17 | #include 18 | #include 19 | #include "nav2_core/exceptions.hpp" 20 | #include "nav_2d_utils/conversions.hpp" 21 | #include "geometry_msgs/msg/pose_stamped.hpp" 22 | #include "geometry_msgs/msg/pose2_d.hpp" 23 | #include "nav2_util/node_utils.hpp" 24 | #include "pluginlib/class_list_macros.hpp" 25 | 26 | using rcl_interfaces::msg::ParameterType; 27 | using std::placeholders::_1; 28 | 29 | namespace mcr_tracking_components 30 | { 31 | static double pose_distance(const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &); 32 | 33 | void FakeProgressChecker::initialize( 34 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, 35 | const std::string & plugin_name) 36 | { 37 | plugin_name_ = plugin_name; 38 | auto node = parent.lock(); 39 | 40 | clock_ = node->get_clock(); 41 | 42 | nav2_util::declare_parameter_if_not_declared( 43 | node, plugin_name + ".required_movement_radius", rclcpp::ParameterValue(0.5)); 44 | nav2_util::declare_parameter_if_not_declared( 45 | node, plugin_name + ".movement_time_allowance", rclcpp::ParameterValue(10.0)); 46 | // Scale is set to 0 by default, so if it was not set otherwise, set to 0 47 | node->get_parameter_or(plugin_name + ".required_movement_radius", radius_, 0.5); 48 | double time_allowance_param = 0.0; 49 | node->get_parameter_or(plugin_name + ".movement_time_allowance", time_allowance_param, 10.0); 50 | time_allowance_ = rclcpp::Duration::from_seconds(time_allowance_param); 51 | 52 | // Setup callback for changes to parameters. 53 | parameters_client_ = std::make_shared( 54 | node->get_node_base_interface(), 55 | node->get_node_topics_interface(), 56 | node->get_node_graph_interface(), 57 | node->get_node_services_interface()); 58 | 59 | parameter_event_sub_ = parameters_client_->on_parameter_event( 60 | std::bind(&FakeProgressChecker::on_parameter_event_callback, this, _1)); 61 | } 62 | 63 | bool FakeProgressChecker::check(geometry_msgs::msg::PoseStamped & /*current_pose*/) 64 | { 65 | return true; 66 | } 67 | 68 | void FakeProgressChecker::reset() 69 | { 70 | baseline_pose_set_ = false; 71 | } 72 | 73 | void FakeProgressChecker::reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose) 74 | { 75 | baseline_pose_ = pose; 76 | baseline_time_ = clock_->now(); 77 | baseline_pose_set_ = true; 78 | } 79 | 80 | bool FakeProgressChecker::is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose) 81 | { 82 | return pose_distance(pose, baseline_pose_) > radius_; 83 | } 84 | 85 | static double pose_distance( 86 | const geometry_msgs::msg::Pose2D & pose1, 87 | const geometry_msgs::msg::Pose2D & pose2) 88 | { 89 | double dx = pose1.x - pose2.x; 90 | double dy = pose1.y - pose2.y; 91 | 92 | return std::hypot(dx, dy); 93 | } 94 | 95 | void 96 | FakeProgressChecker::on_parameter_event_callback( 97 | const rcl_interfaces::msg::ParameterEvent::SharedPtr event) 98 | { 99 | for (auto & changed_parameter : event->changed_parameters) { 100 | const auto & type = changed_parameter.value.type; 101 | const auto & name = changed_parameter.name; 102 | const auto & value = changed_parameter.value; 103 | 104 | if (type == ParameterType::PARAMETER_DOUBLE) { 105 | if (name == plugin_name_ + ".required_movement_radius") { 106 | radius_ = value.double_value; 107 | } else if (name == plugin_name_ + ".movement_time_allowance") { 108 | time_allowance_ = rclcpp::Duration::from_seconds(value.double_value); 109 | } 110 | } 111 | } 112 | } 113 | 114 | } // namespace mcr_tracking_components 115 | 116 | PLUGINLIB_EXPORT_CLASS(mcr_tracking_components::FakeProgressChecker, nav2_core::ProgressChecker) 117 | -------------------------------------------------------------------------------- /mcr_tracking_components/src/controller_plugins/unreachable_goal_checker.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 "mcr_tracking_components/controller_plugins/unreachable_goal_checker.hpp" 19 | #include "pluginlib/class_list_macros.hpp" 20 | #include "angles/angles.h" 21 | #include "nav2_util/node_utils.hpp" 22 | #include "nav2_util/geometry_utils.hpp" 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wpedantic" 25 | #include "tf2/utils.h" 26 | #pragma GCC diagnostic pop 27 | 28 | using rcl_interfaces::msg::ParameterType; 29 | using std::placeholders::_1; 30 | 31 | namespace mcr_tracking_components 32 | { 33 | 34 | UnreachableGoalChecker::UnreachableGoalChecker() 35 | : xy_goal_tolerance_(0.25), 36 | yaw_goal_tolerance_(0.25), 37 | stateful_(true), 38 | check_xy_(true), 39 | xy_goal_tolerance_sq_(0.0625) 40 | { 41 | } 42 | 43 | void UnreachableGoalChecker::initialize( 44 | const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, 45 | const std::string & plugin_name) 46 | { 47 | plugin_name_ = plugin_name; 48 | auto node = parent.lock(); 49 | 50 | // Setup callback for changes to parameters. 51 | parameters_client_ = std::make_shared( 52 | node->get_node_base_interface(), 53 | node->get_node_topics_interface(), 54 | node->get_node_graph_interface(), 55 | node->get_node_services_interface()); 56 | 57 | parameter_event_sub_ = parameters_client_->on_parameter_event( 58 | std::bind(&UnreachableGoalChecker::on_parameter_event_callback, this, _1)); 59 | } 60 | 61 | void UnreachableGoalChecker::reset() 62 | { 63 | } 64 | 65 | bool UnreachableGoalChecker::isGoalReached( 66 | const geometry_msgs::msg::Pose & , const geometry_msgs::msg::Pose & , 67 | const geometry_msgs::msg::Twist &) 68 | { 69 | return false; 70 | } 71 | 72 | bool UnreachableGoalChecker::getTolerances( 73 | geometry_msgs::msg::Pose & , 74 | geometry_msgs::msg::Twist & ) 75 | { 76 | return true; 77 | } 78 | 79 | void 80 | UnreachableGoalChecker::on_parameter_event_callback( 81 | const rcl_interfaces::msg::ParameterEvent::SharedPtr ) 82 | { 83 | } 84 | 85 | } // namespace mcr_tracking_components 86 | 87 | PLUGINLIB_EXPORT_CLASS(mcr_tracking_components::UnreachableGoalChecker, nav2_core::GoalChecker) 88 | -------------------------------------------------------------------------------- /mcr_tracking_components/src/dwb_critics/keep_person_insight.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 "mcr_tracking_components/dwb_critics/keep_person_insight.hpp" 16 | 17 | #include 18 | #include 19 | 20 | #include "angles/angles.h" 21 | #include "dwb_core/exceptions.hpp" 22 | #include "dwb_core/trajectory_utils.hpp" 23 | #include "angles/angles.h" 24 | #include 25 | #include 26 | #include "tf2_ros/transform_broadcaster.h" 27 | #include 28 | #include "tf2_ros/transform_listener.h" 29 | #include "tf2/utils.h" 30 | #include "tf2/LinearMath/Transform.h" 31 | #include "tf2_ros/buffer.h" 32 | #include "tf2_ros/buffer_interface.h" 33 | #include "tf2/buffer_core.h" 34 | #include "tf2_ros/message_filter.h" 35 | #include "tf2_ros/create_timer_ros.h" 36 | #include "nav2_util/robot_utils.hpp" 37 | #include "nav_2d_utils/parameters.hpp" 38 | #include "pluginlib/class_list_macros.hpp" 39 | 40 | PLUGINLIB_EXPORT_CLASS(mcr_tracking_components::KeepPersonInsightCritic, 41 | dwb_core::TrajectoryCritic) 42 | 43 | namespace mcr_tracking_components { 44 | 45 | inline double hypot_sq(double dx, double dy) { return dx * dx + dy * dy; } 46 | 47 | void KeepPersonInsightCritic::onInit() { 48 | normal_sacle_ = 0.0; 49 | inuse_ = true; 50 | rcl_node_ = node_.lock(); 51 | if (!rcl_node_) { 52 | throw std::runtime_error{"Failed to lock node"}; 53 | } 54 | nav2_util::declare_parameter_if_not_declared(rcl_node_, dwb_plugin_name_ + "." + name_ + ".target_topic", 55 | rclcpp::ParameterValue("person")); 56 | rcl_node_->get_parameter(dwb_plugin_name_ + "." + name_ + ".target_topic", target_topic_); 57 | 58 | lookahead_time_ = nav_2d_utils::searchAndGetParam( 59 | rcl_node_, dwb_plugin_name_ + "." + name_ + ".lookahead_time", -1.0); 60 | 61 | 62 | tf_buffer_ = std::make_shared(rcl_node_->get_clock()); 63 | auto timer_interface = std::make_shared( 64 | rcl_node_->get_node_base_interface(), 65 | rcl_node_->get_node_timers_interface()); 66 | tf_buffer_->setCreateTimerInterface(timer_interface); 67 | tf_listener_ = std::make_shared(*tf_buffer_); 68 | 69 | pose_sub_ = rcl_node_->create_subscription( 70 | target_topic_, 71 | rclcpp::SensorDataQoS(), 72 | std::bind(&KeepPersonInsightCritic::poseCallback, this, std::placeholders::_1)); 73 | RCLCPP_INFO( 74 | rcl_node_->get_logger(), 75 | "Keep target insight critic subscribed to tracking pose: tracking_pose"); 76 | 77 | reset(); 78 | normal_sacle_ = getScale(); 79 | } 80 | 81 | void KeepPersonInsightCritic::reset() {} 82 | 83 | bool KeepPersonInsightCritic::prepare( 84 | const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & /*vel*/, 85 | const geometry_msgs::msg::Pose2D & /*goal*/, 86 | const nav_2d_msgs::msg::Path2D &) 87 | { 88 | cur_yaw_ = pose.theta; 89 | goal_yaw_ = (320.0 - (latest_person_.track_res.roi.x_offset + latest_person_.track_res.roi.width / 2.0)) * 0.0027; 90 | return true; 91 | } 92 | 93 | double KeepPersonInsightCritic::scoreTrajectory( 94 | const dwb_msgs::msg::Trajectory2D &traj) { 95 | if((rcl_node_->now().seconds() - latest_timestamp_.seconds()) > 2.5){ 96 | // RCLCPP_INFO(rcl_node_->get_logger(), 97 | // "Keep person insight critic timed out for subscriber: person"); 98 | return 0.0; 99 | } 100 | 101 | return scoreRotation(traj); 102 | } 103 | 104 | double KeepPersonInsightCritic::scoreRotation( 105 | const dwb_msgs::msg::Trajectory2D &traj) { 106 | if (traj.poses.empty()) { 107 | throw dwb_core::IllegalTrajectoryException(name_, "Empty trajectory."); 108 | } 109 | 110 | double end_yaw; 111 | if (lookahead_time_ >= 0.0) { 112 | geometry_msgs::msg::Pose2D eval_pose = 113 | dwb_core::projectPose(traj, lookahead_time_); 114 | end_yaw = eval_pose.theta; 115 | } else { 116 | end_yaw = traj.poses.back().theta; 117 | } 118 | return fabs(angles::shortest_angular_distance(end_yaw - cur_yaw_, goal_yaw_)); 119 | } 120 | 121 | void KeepPersonInsightCritic::poseCallback(const protocol::msg::Person::SharedPtr msg) 122 | { 123 | if(msg->track_res.roi.height == 0 || msg->track_res.roi.width == 0) return; 124 | 125 | latest_person_ = *msg; 126 | latest_timestamp_ = rcl_node_->now(); 127 | } 128 | 129 | } // namespace mcr_tracking_components 130 | -------------------------------------------------------------------------------- /mcr_uwb/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mcr_uwb) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(rclcpp REQUIRED) 6 | find_package(rclcpp_lifecycle REQUIRED) 7 | find_package(nav2_util REQUIRED) 8 | find_package(geometry_msgs REQUIRED) 9 | find_package(builtin_interfaces REQUIRED) 10 | find_package(tf2_ros REQUIRED) 11 | find_package(protocol REQUIRED) 12 | find_package(cyberdog_debug REQUIRED) 13 | find_package(cyberdog_common REQUIRED) 14 | include_directories( 15 | include 16 | ) 17 | 18 | set(executable_name mcr_uwb) 19 | set(library_name ${executable_name}_core) 20 | 21 | set(dependencies 22 | rclcpp 23 | rclcpp_lifecycle 24 | nav2_util 25 | protocol 26 | geometry_msgs 27 | builtin_interfaces 28 | tf2_ros 29 | cyberdog_debug 30 | cyberdog_common 31 | ) 32 | 33 | add_library(${library_name} SHARED 34 | src/mcr_uwb.cpp 35 | ) 36 | 37 | ament_target_dependencies(${library_name} 38 | ${dependencies} 39 | ) 40 | 41 | target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 42 | 43 | add_executable(${executable_name} 44 | src/main.cpp 45 | ) 46 | 47 | target_link_libraries(${executable_name} ${library_name}) 48 | 49 | ament_target_dependencies(${executable_name} 50 | ${dependencies} 51 | ) 52 | 53 | # prevent pluginlib from using boost 54 | target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 55 | 56 | install(TARGETS ${library_name} 57 | ARCHIVE DESTINATION lib 58 | LIBRARY DESTINATION lib 59 | RUNTIME DESTINATION bin 60 | ) 61 | 62 | install(TARGETS ${executable_name} 63 | RUNTIME DESTINATION lib/${PROJECT_NAME} 64 | ) 65 | 66 | install(DIRECTORY include/ 67 | DESTINATION include/ 68 | ) 69 | 70 | install( 71 | DIRECTORY launch 72 | DESTINATION share/${PROJECT_NAME} 73 | ) 74 | 75 | if(BUILD_TESTING) 76 | find_package(ament_lint_auto REQUIRED) 77 | ament_lint_auto_find_test_dependencies() 78 | endif() 79 | 80 | ament_export_include_directories(include) 81 | ament_export_libraries(${library_name}) 82 | ament_export_dependencies(${dependencies}) 83 | ament_package() 84 | -------------------------------------------------------------------------------- /mcr_uwb/README.md: -------------------------------------------------------------------------------- 1 | #mcr_uwb 2 | 3 | UWB原始数据协议如下： 4 | ``` 5 | topic name: uwb_raw 6 | topic type: protocol/msg/UwbRaw.msg 7 | ``` 8 | 9 | UwbRaw: 10 | ``` 11 | std_msgs/Header header 12 | 13 | float32 dist //m 14 | float32 angle //rad 15 | float32 n_los 16 | float32 rssi_1 17 | float32 rssi_2 18 | ``` 19 | 20 | UwbRaw原始数据转换为PoseStamped: 21 | ``` 22 | geometry_msgs::msg::PoseStamped pose; 23 | pose.header = uwb->header; 24 | 25 | RCLCPP_DEBUG( 26 | get_logger(), "received uwb raw data, frame id: %s, dist: %f, angle: %f.", 27 | uwb->header.frame_id.c_str(), uwb->dist, uwb->angle); 28 | 29 | pose.pose.position.x = uwb->dist * cos(-uwb->angle); 30 | pose.pose.position.y = uwb->dist * sin(-uwb->angle); 31 | 32 | ``` 33 | 34 | 数据转换完成之后，在后续数据使用上，根据位置关系利用tf transformer进行调整。 35 | -------------------------------------------------------------------------------- /mcr_uwb/include/mcr_uwb/mcr_uwb.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MCR_UWB__MCR_UWB_HPP_ 16 | #define MCR_UWB__MCR_UWB_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "geometry_msgs/msg/pose_stamped.hpp" 25 | #include "nav2_util/lifecycle_node.hpp" 26 | #include "nav2_util/robot_utils.hpp" 27 | #include "tf2_ros/transform_listener.h" 28 | #include "tf2_ros/create_timer_ros.h" 29 | #include "protocol/msg/uwb_raw.hpp" 30 | 31 | namespace mcr_uwb 32 | { 33 | /** 34 | * @class mcr_uwb::MCRUwb 35 | * @brief a transform for uwb raw data to posestamp 36 | */ 37 | class MCRUwb : public nav2_util::LifecycleNode 38 | { 39 | public: 40 | /** 41 | * @brief A constructor for mcr_planner::MCRUwb 42 | */ 43 | MCRUwb(); 44 | /** 45 | * @brief A destructor for mcr_planner::MCRUwb 46 | */ 47 | ~MCRUwb(); 48 | 49 | protected: 50 | /** 51 | * @brief Configure member variables and initializes planner 52 | * @param state Reference to LifeCycle node state 53 | * @return SUCCESS or FAILURE 54 | */ 55 | nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; 56 | /** 57 | * @brief Activate member variables 58 | * @param state Reference to LifeCycle node state 59 | * @return SUCCESS or FAILURE 60 | */ 61 | nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; 62 | /** 63 | * @brief Deactivate member variables 64 | * @param state Reference to LifeCycle node state 65 | * @return SUCCESS or FAILURE 66 | */ 67 | nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; 68 | /** 69 | * @brief Reset member variables 70 | * @param state Reference to LifeCycle node state 71 | * @return SUCCESS or FAILURE 72 | */ 73 | nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; 74 | /** 75 | * @brief Called when in shutdown state 76 | * @param state Reference to LifeCycle node state 77 | * @return SUCCESS or FAILURE 78 | */ 79 | nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; 80 | /** 81 | * @brief callback for uwb msgs 82 | * @param uwb sharedptr for uwb raw data 83 | */ 84 | void incomingUwb(protocol::msg::UwbRaw::ConstSharedPtr uwb); 85 | 86 | private: 87 | // Publishers for the path 88 | rclcpp_lifecycle::LifecyclePublisher::SharedPtr pose_pub_; 89 | rclcpp::Subscription::SharedPtr uwb_sub_; 90 | }; 91 | 92 | } // namespace mcr_uwb 93 | 94 | #endif // MCR_UWB__MCR_UWB_HPP_ 95 | -------------------------------------------------------------------------------- /mcr_uwb/launch/uwb_launch.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 | from launch import LaunchDescription 15 | from launch.actions import SetEnvironmentVariable 16 | from launch_ros.actions import Node 17 | 18 | 19 | def generate_launch_description(): 20 | lifecycle_nodes = ['mcr_uwb'] 21 | 22 | return LaunchDescription([ 23 | # Set env var to print messages to stdout immediately 24 | SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), 25 | Node( 26 | package='mcr_uwb', 27 | executable='mcr_uwb', 28 | name='mcr_uwb', 29 | # prefix=['xterm -e gdb --args'] 30 | ), 31 | Node( 32 | package='tf2_ros', 33 | executable='static_transform_publisher', 34 | parameters=[{'use_sim_time': True}], 35 | arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'uwb']), 36 | Node( 37 | package='tf2_ros', 38 | executable='static_transform_publisher', 39 | parameters=[{'use_sim_time': True}], 40 | arguments=['0.2185', '0', '-0.00495', '0', '0', '0', 'uwb', 'head_uwb']), 41 | 42 | Node( 43 | package='tf2_ros', 44 | executable='static_transform_publisher', 45 | parameters=[{'use_sim_time': True}], 46 | arguments=['0.17', '0', '0.164', '3.14159', '0', '0', 'uwb', 'head_tof']), 47 | 48 | Node( 49 | package='tf2_ros', 50 | executable='static_transform_publisher', 51 | parameters=[{'use_sim_time': True}], 52 | arguments=['-0.023', '0.0845', '-0.00325', '1.5708', '0', '0', 'uwb', 'rear_uwb']), 53 | 54 | Node( 55 | package='tf2_ros', 56 | executable='static_transform_publisher', 57 | parameters=[{'use_sim_time': True}], 58 | arguments=['-0.0235', '-0.0845', '-0.00325', '-1.5708', '0', '0', 'uwb', 'rear_tof']), 59 | 60 | Node( 61 | package='nav2_lifecycle_manager', 62 | executable='lifecycle_manager', 63 | name='lifecycle_manager_simulator', 64 | output='screen', 65 | parameters=[{'use_sim_time': True}, 66 | {'autostart': True}, 67 | {'node_names': lifecycle_nodes}]) 68 | ]) 69 | -------------------------------------------------------------------------------- /mcr_uwb/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mcr_uwb 5 | 1.0.0 6 | TODO 7 | zhangchuanfa 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_lifecycle 14 | nav2_util 15 | protocol 16 | geometry_msgs 17 | builtin_interfaces 18 | tf2_ros 19 | cyberdog_debug 20 | cyberdog_common 21 | ament_lint_common 22 | ament_lint_auto 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /mcr_uwb/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include 15 | #include "cyberdog_debug/backtrace.hpp" 16 | #include "mcr_uwb/mcr_uwb.hpp" 17 | #include "rclcpp/rclcpp.hpp" 18 | 19 | int main(int argc, char ** argv) 20 | { 21 | cyberdog::debug::register_signal(); 22 | rclcpp::init(argc, argv); 23 | auto node = std::make_shared(); 24 | rclcpp::spin(node->get_node_base_interface()); 25 | rclcpp::shutdown(); 26 | 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /mcr_uwb/src/mcr_uwb.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 "mcr_uwb/mcr_uwb.hpp" 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include "builtin_interfaces/msg/duration.hpp" 26 | #include "nav2_util/costmap.hpp" 27 | 28 | using namespace std::chrono_literals; 29 | 30 | namespace mcr_uwb 31 | { 32 | 33 | MCRUwb::MCRUwb() 34 | : nav2_util::LifecycleNode("mcr_uwb", "", true) 35 | { 36 | RCLCPP_INFO(get_logger(), "Creating"); 37 | 38 | // Declare this node's parameters 39 | declare_parameter("uwb_data", "uwb_raw"); 40 | declare_parameter("produced_pose", "tracking_pose"); 41 | } 42 | 43 | MCRUwb::~MCRUwb() 44 | { 45 | } 46 | 47 | nav2_util::CallbackReturn 48 | MCRUwb::on_configure(const rclcpp_lifecycle::State &) 49 | { 50 | RCLCPP_INFO(get_logger(), "Configuring"); 51 | auto node = shared_from_this(); 52 | std::string uwb_data_topic, produced_pose; 53 | get_parameter("uwb_data", uwb_data_topic); 54 | get_parameter("produced_pose", produced_pose); 55 | 56 | // Initialize pubs & subs 57 | pose_pub_ = create_publisher(produced_pose, 5); 58 | 59 | uwb_sub_ = create_subscription( 60 | uwb_data_topic, 10, 61 | std::bind(&MCRUwb::incomingUwb, this, std::placeholders::_1)); 62 | 63 | 64 | return nav2_util::CallbackReturn::SUCCESS; 65 | } 66 | 67 | nav2_util::CallbackReturn 68 | MCRUwb::on_activate(const rclcpp_lifecycle::State &) 69 | { 70 | RCLCPP_INFO(get_logger(), "Activating"); 71 | 72 | pose_pub_->on_activate(); 73 | // uwb_sub_->on_activate(); 74 | 75 | // create bond connection 76 | createBond(); 77 | 78 | return nav2_util::CallbackReturn::SUCCESS; 79 | } 80 | 81 | nav2_util::CallbackReturn 82 | MCRUwb::on_deactivate(const rclcpp_lifecycle::State &) 83 | { 84 | RCLCPP_INFO(get_logger(), "Deactivating"); 85 | 86 | pose_pub_->on_deactivate(); 87 | // uwb_sub_->on_deactivate(); 88 | 89 | // destroy bond connection 90 | destroyBond(); 91 | 92 | return nav2_util::CallbackReturn::SUCCESS; 93 | } 94 | 95 | nav2_util::CallbackReturn 96 | MCRUwb::on_cleanup(const rclcpp_lifecycle::State & /*state*/) 97 | { 98 | RCLCPP_INFO(get_logger(), "Cleaning up"); 99 | 100 | pose_pub_.reset(); 101 | uwb_sub_.reset(); 102 | return nav2_util::CallbackReturn::SUCCESS; 103 | } 104 | 105 | nav2_util::CallbackReturn 106 | MCRUwb::on_shutdown(const rclcpp_lifecycle::State &) 107 | { 108 | RCLCPP_INFO(get_logger(), "Shutting down"); 109 | return nav2_util::CallbackReturn::SUCCESS; 110 | } 111 | 112 | 113 | void 114 | MCRUwb::incomingUwb(protocol::msg::UwbRaw::ConstSharedPtr uwb) 115 | { 116 | if (!pose_pub_->is_activated()) { 117 | return; 118 | } 119 | geometry_msgs::msg::PoseStamped pose; 120 | pose.header = uwb->header; 121 | 122 | RCLCPP_DEBUG( 123 | get_logger(), "received uwb raw data, frame id: %s, dist: %f, angle: %f.", 124 | uwb->header.frame_id.c_str(), uwb->dist, uwb->angle); 125 | 126 | pose.pose.position.x = uwb->dist * cos(-uwb->angle); 127 | pose.pose.position.y = uwb->dist * sin(-uwb->angle); 128 | 129 | pose.pose.orientation.w = 1.0; 130 | 131 | pose_pub_->publish(pose); 132 | } 133 | 134 | } // namespace mcr_uwb 135 | -------------------------------------------------------------------------------- /mcr_voice/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mcr_voice) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(rclcpp REQUIRED) 6 | find_package(rclcpp_lifecycle REQUIRED) 7 | find_package(nav2_util REQUIRED) 8 | find_package(nav2_core REQUIRED) 9 | find_package(geometry_msgs REQUIRED) 10 | find_package(mcr_msgs REQUIRED) 11 | find_package(builtin_interfaces REQUIRED) 12 | find_package(tf2_ros REQUIRED) 13 | find_package(protocol REQUIRED) 14 | find_package(cyberdog_debug REQUIRED) 15 | find_package(cyberdog_common REQUIRED) 16 | include_directories( 17 | include 18 | ) 19 | 20 | set(executable_name mcr_voice) 21 | set(library_name ${executable_name}_core) 22 | 23 | set(dependencies 24 | rclcpp 25 | rclcpp_lifecycle 26 | nav2_util 27 | nav2_core 28 | protocol 29 | geometry_msgs 30 | mcr_msgs 31 | builtin_interfaces 32 | tf2_ros 33 | cyberdog_debug 34 | cyberdog_common 35 | ) 36 | 37 | add_library(${library_name} SHARED 38 | src/mcr_voice.cpp 39 | ) 40 | 41 | ament_target_dependencies(${library_name} 42 | ${dependencies} 43 | ) 44 | 45 | target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 46 | 47 | add_executable(${executable_name} 48 | src/main.cpp 49 | ) 50 | 51 | target_link_libraries(${executable_name} ${library_name}) 52 | 53 | ament_target_dependencies(${executable_name} 54 | ${dependencies} 55 | ) 56 | 57 | # prevent pluginlib from using boost 58 | target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 59 | 60 | install(TARGETS ${library_name} 61 | ARCHIVE DESTINATION lib 62 | LIBRARY DESTINATION lib 63 | RUNTIME DESTINATION bin 64 | ) 65 | 66 | install(TARGETS ${executable_name} 67 | RUNTIME DESTINATION lib/${PROJECT_NAME} 68 | ) 69 | 70 | install(DIRECTORY include/ 71 | DESTINATION include/ 72 | ) 73 | 74 | install( 75 | DIRECTORY launch 76 | DESTINATION share/${PROJECT_NAME} 77 | ) 78 | 79 | if(BUILD_TESTING) 80 | find_package(ament_lint_auto REQUIRED) 81 | ament_lint_auto_find_test_dependencies() 82 | endif() 83 | 84 | ament_export_include_directories(include) 85 | ament_export_libraries(${library_name}) 86 | ament_export_dependencies(${dependencies}) 87 | ament_package() -------------------------------------------------------------------------------- /mcr_voice/README.md: -------------------------------------------------------------------------------- 1 | # mcr_voice 2 | 3 | 根据跟随任务时action server的feedback数据，进行判断当前跟随状态，并进行语音播报。 4 | 语音播报是以audio service的接口形式进行调用。 -------------------------------------------------------------------------------- /mcr_voice/include/mcr_voice/mcr_voice.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 MCR_VOICE__MCR_VOICE_HPP_ 15 | #define MCR_VOICE__MCR_VOICE_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "geometry_msgs/msg/pose_stamped.hpp" 24 | #include "nav2_util/lifecycle_node.hpp" 25 | #include "nav2_util/robot_utils.hpp" 26 | #include "tf2_ros/transform_listener.h" 27 | #include "tf2_ros/create_timer_ros.h" 28 | #include "mcr_msgs/action/detail/target_tracking__struct.hpp" 29 | #include "protocol/srv/audio_text_play.hpp" 30 | #include "protocol/msg/audio_play.hpp" 31 | 32 | namespace mcr_voice 33 | { 34 | /** 35 | * @class mcr_voice::MCRVoice 36 | * @brief a transform for uwb raw data to posestamp 37 | */ 38 | class MCRVoice : public rclcpp::Node 39 | { 40 | public: 41 | /** 42 | * @brief A constructor for mcr_planner::MCRVoice 43 | */ 44 | MCRVoice(); 45 | /** 46 | * @brief A destructor for mcr_planner::MCRVoice 47 | */ 48 | ~MCRVoice(); 49 | 50 | protected: 51 | /** 52 | * @brief call audio play server 53 | * @param audio content to play 54 | */ 55 | void playAudio(const std::string& audio); 56 | /** 57 | * @brief subscribe action server's feedback for audio tracking status 58 | * @param feedback for tracking status 59 | */ 60 | void incomingFeedback(mcr_msgs::action::TargetTracking_FeedbackMessage::ConstSharedPtr feedback); 61 | private: 62 | rclcpp::Subscription::SharedPtr feedback_sub_; 63 | rclcpp::Client::SharedPtr audio_play_client_; 64 | double valid_range_; 65 | rclcpp::CallbackGroup::SharedPtr callback_group_; 66 | }; 67 | 68 | } // namespace mcr_voice 69 | 70 | #endif // MCR_VOICE__MCR_VOICE_HPP_ 71 | -------------------------------------------------------------------------------- /mcr_voice/launch/voice_launch.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 | from launch import LaunchDescription 15 | from launch.actions import SetEnvironmentVariable 16 | from launch_ros.actions import Node 17 | 18 | 19 | def generate_launch_description(): 20 | lifecycle_nodes = ['mcr_voice'] 21 | 22 | return LaunchDescription([ 23 | # Set env var to print messages to stdout immediately 24 | SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), 25 | Node( 26 | package='mcr_voice', 27 | executable='mcr_voice', 28 | name='mcr_voice', 29 | # prefix=['xterm -e gdb --args'] 30 | ), 31 | Node( 32 | package='nav2_lifecycle_manager', 33 | executable='lifecycle_manager', 34 | name='lifecycle_manager_voice', 35 | output='screen', 36 | parameters=[{'use_sim_time': True}, 37 | {'autostart': True}, 38 | {'node_names': lifecycle_nodes}]) 39 | ]) 40 | -------------------------------------------------------------------------------- /mcr_voice/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mcr_voice 5 | 1.0.0 6 | TODO 7 | zhangchuanfa 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_lifecycle 14 | nav2_util 15 | nav2_core 16 | protocol 17 | geometry_msgs 18 | mcr_msgs 19 | builtin_interfaces 20 | tf2_ros 21 | cyberdog_debug 22 | cyberdog_common 23 | ament_lint_common 24 | ament_lint_auto 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /mcr_voice/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include 15 | #include "cyberdog_debug/backtrace.hpp" 16 | #include "mcr_voice/mcr_voice.hpp" 17 | #include "rclcpp/rclcpp.hpp" 18 | 19 | int main(int argc, char ** argv) 20 | { 21 | cyberdog::debug::register_signal(); 22 | rclcpp::init(argc, argv); 23 | auto node = std::make_shared(); 24 | rclcpp::spin(node->get_node_base_interface()); 25 | rclcpp::shutdown(); 26 | 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /mcr_voice/src/mcr_voice.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 "mcr_voice/mcr_voice.hpp" 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include "builtin_interfaces/msg/duration.hpp" 26 | #include "nav2_util/costmap.hpp" 27 | #include "nav2_core/exceptions.hpp" 28 | 29 | 30 | using namespace std::chrono_literals; 31 | 32 | namespace mcr_voice 33 | { 34 | 35 | MCRVoice::MCRVoice() 36 | : rclcpp::Node("mcr_voice") 37 | { 38 | RCLCPP_INFO(get_logger(), "Creating"); 39 | 40 | // Declare this node's parameters 41 | declare_parameter("feedback_topic", "tracking_target/_action/feedback"); 42 | declare_parameter("valid_range", 3.5); 43 | callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); 44 | std::string feedback_topic, produced_pose; 45 | get_parameter("feedback_topic", feedback_topic); 46 | get_parameter("valid_range", valid_range_); 47 | 48 | 49 | audio_play_client_ = create_client( 50 | "speech_text_play", 51 | rmw_qos_profile_services_default, 52 | callback_group_); 53 | 54 | feedback_sub_ = create_subscription( 55 | feedback_topic, 1, 56 | std::bind(&MCRVoice::incomingFeedback, this, std::placeholders::_1)); 57 | } 58 | 59 | MCRVoice::~MCRVoice() 60 | { 61 | feedback_sub_.reset(); 62 | } 63 | 64 | 65 | void MCRVoice::playAudio(const std::string & audio) 66 | { 67 | auto request = std::make_shared(); 68 | request->module_name = get_name(); 69 | request->is_online = true; 70 | request->text = audio; 71 | auto callback = [&](rclcpp::Client::SharedFuture future) { 72 | RCLCPP_INFO( 73 | get_logger(), "Audio play result: %s", 74 | future.get()->status == 0 ? "success" : "failed"); 75 | }; 76 | auto future_online = audio_play_client_->async_send_request(request, callback); 77 | if (future_online.wait_for(std::chrono::milliseconds(3000)) == std::future_status::timeout) { 78 | RCLCPP_ERROR(get_logger(), "Cannot get response of AudioPlay"); 79 | } 80 | } 81 | 82 | 83 | void 84 | MCRVoice::incomingFeedback( 85 | mcr_msgs::action::TargetTracking_FeedbackMessage::ConstSharedPtr feedbackmsg) 86 | { 87 | static unsigned int k = 1; 88 | if (feedbackmsg->feedback.exception_code == nav2_core::DETECTOREXCEPTION) { 89 | RCLCPP_INFO(get_logger(), "The target is out of sight. Voice prompts are needed."); 90 | k += 1; 91 | if (k % 5 == 0) { 92 | playAudio("主人, 我找不到你了，不要丢下我。"); 93 | } 94 | return; 95 | } 96 | if (feedbackmsg->feedback.current_distance > valid_range_) { 97 | RCLCPP_INFO(get_logger(), "It is too far away from the target and needs voice prompt."); 98 | k += 1; 99 | if (k % 5 == 0) { 100 | playAudio("主人，等等我，跟不上你了。"); 101 | } 102 | return; 103 | } 104 | if (feedbackmsg->feedback.exception_code == nav2_core::PLANNEREXECPTION || 105 | feedbackmsg->feedback.exception_code == nav2_core::CONTROLLEREXECPTION) 106 | { 107 | RCLCPP_INFO(get_logger(), "Something is wrong with planning. Voice prompts are needed."); 108 | playAudio("主人，我遇到了一些困难，帮帮我好吗？"); 109 | } 110 | } 111 | 112 | } // namespace mcr_voice 113 | --------------------------------------------------------------------------------