├── .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