add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,240 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(nav2_behavior_tree CXX)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(nav2_common REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(rclcpp_lifecycle REQUIRED)
|
||||
find_package(builtin_interfaces REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(nav2_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(behaviortree_cpp_v3 REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(tf2_geometry_msgs REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(std_srvs REQUIRED)
|
||||
find_package(nav2_util REQUIRED)
|
||||
|
||||
nav2_package()
|
||||
|
||||
include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
set(library_name ${PROJECT_NAME})
|
||||
|
||||
set(dependencies
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
rclcpp_lifecycle
|
||||
geometry_msgs
|
||||
sensor_msgs
|
||||
nav2_msgs
|
||||
nav_msgs
|
||||
behaviortree_cpp_v3
|
||||
tf2
|
||||
tf2_ros
|
||||
tf2_geometry_msgs
|
||||
std_msgs
|
||||
std_srvs
|
||||
nav2_util
|
||||
)
|
||||
|
||||
add_library(${library_name} SHARED
|
||||
src/behavior_tree_engine.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(${library_name}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
add_library(nav2_compute_path_to_pose_action_bt_node SHARED plugins/action/compute_path_to_pose_action.cpp)
|
||||
list(APPEND plugin_libs nav2_compute_path_to_pose_action_bt_node)
|
||||
|
||||
add_library(nav2_compute_path_through_poses_action_bt_node SHARED plugins/action/compute_path_through_poses_action.cpp)
|
||||
list(APPEND plugin_libs nav2_compute_path_through_poses_action_bt_node)
|
||||
|
||||
add_library(nav2_controller_cancel_bt_node SHARED plugins/action/controller_cancel_node.cpp)
|
||||
list(APPEND plugin_libs nav2_controller_cancel_bt_node)
|
||||
|
||||
add_library(nav2_wait_cancel_bt_node SHARED plugins/action/wait_cancel_node.cpp)
|
||||
list(APPEND plugin_libs nav2_wait_cancel_bt_node)
|
||||
|
||||
add_library(nav2_spin_cancel_bt_node SHARED plugins/action/spin_cancel_node.cpp)
|
||||
list(APPEND plugin_libs nav2_spin_cancel_bt_node)
|
||||
|
||||
add_library(nav2_back_up_cancel_bt_node SHARED plugins/action/back_up_cancel_node.cpp)
|
||||
list(APPEND plugin_libs nav2_back_up_cancel_bt_node)
|
||||
|
||||
add_library(nav2_assisted_teleop_cancel_bt_node SHARED plugins/action/assisted_teleop_cancel_node.cpp)
|
||||
list(APPEND plugin_libs nav2_assisted_teleop_cancel_bt_node)
|
||||
|
||||
add_library(nav2_drive_on_heading_cancel_bt_node SHARED plugins/action/drive_on_heading_cancel_node.cpp)
|
||||
list(APPEND plugin_libs nav2_drive_on_heading_cancel_bt_node)
|
||||
|
||||
add_library(nav2_smooth_path_action_bt_node SHARED plugins/action/smooth_path_action.cpp)
|
||||
list(APPEND plugin_libs nav2_smooth_path_action_bt_node)
|
||||
|
||||
add_library(nav2_follow_path_action_bt_node SHARED plugins/action/follow_path_action.cpp)
|
||||
list(APPEND plugin_libs nav2_follow_path_action_bt_node)
|
||||
|
||||
add_library(nav2_back_up_action_bt_node SHARED plugins/action/back_up_action.cpp)
|
||||
list(APPEND plugin_libs nav2_back_up_action_bt_node)
|
||||
|
||||
add_library(nav2_drive_on_heading_bt_node SHARED plugins/action/drive_on_heading_action.cpp)
|
||||
list(APPEND plugin_libs nav2_drive_on_heading_bt_node)
|
||||
|
||||
add_library(nav2_spin_action_bt_node SHARED plugins/action/spin_action.cpp)
|
||||
list(APPEND plugin_libs nav2_spin_action_bt_node)
|
||||
|
||||
add_library(nav2_wait_action_bt_node SHARED plugins/action/wait_action.cpp)
|
||||
list(APPEND plugin_libs nav2_wait_action_bt_node)
|
||||
|
||||
add_library(nav2_assisted_teleop_action_bt_node SHARED plugins/action/assisted_teleop_action.cpp)
|
||||
list(APPEND plugin_libs nav2_assisted_teleop_action_bt_node)
|
||||
|
||||
add_library(nav2_clear_costmap_service_bt_node SHARED plugins/action/clear_costmap_service.cpp)
|
||||
list(APPEND plugin_libs nav2_clear_costmap_service_bt_node)
|
||||
|
||||
add_library(nav2_is_stuck_condition_bt_node SHARED plugins/condition/is_stuck_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_is_stuck_condition_bt_node)
|
||||
|
||||
add_library(nav2_transform_available_condition_bt_node SHARED plugins/condition/transform_available_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_transform_available_condition_bt_node)
|
||||
|
||||
add_library(nav2_goal_reached_condition_bt_node SHARED plugins/condition/goal_reached_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_goal_reached_condition_bt_node)
|
||||
|
||||
add_library(nav2_globally_updated_goal_condition_bt_node SHARED plugins/condition/globally_updated_goal_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_globally_updated_goal_condition_bt_node)
|
||||
|
||||
add_library(nav2_goal_updated_condition_bt_node SHARED plugins/condition/goal_updated_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_goal_updated_condition_bt_node)
|
||||
|
||||
add_library(nav2_is_path_valid_condition_bt_node SHARED plugins/condition/is_path_valid_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_is_path_valid_condition_bt_node)
|
||||
|
||||
add_library(nav2_time_expired_condition_bt_node SHARED plugins/condition/time_expired_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_time_expired_condition_bt_node)
|
||||
|
||||
add_library(nav2_path_expiring_timer_condition SHARED plugins/condition/path_expiring_timer_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_path_expiring_timer_condition)
|
||||
|
||||
add_library(nav2_distance_traveled_condition_bt_node SHARED plugins/condition/distance_traveled_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_distance_traveled_condition_bt_node)
|
||||
|
||||
add_library(nav2_initial_pose_received_condition_bt_node SHARED plugins/condition/initial_pose_received_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_initial_pose_received_condition_bt_node)
|
||||
|
||||
add_library(nav2_is_battery_charging_condition_bt_node SHARED plugins/condition/is_battery_charging_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_is_battery_charging_condition_bt_node)
|
||||
|
||||
add_library(nav2_is_battery_low_condition_bt_node SHARED plugins/condition/is_battery_low_condition.cpp)
|
||||
list(APPEND plugin_libs nav2_is_battery_low_condition_bt_node)
|
||||
|
||||
add_library(nav2_reinitialize_global_localization_service_bt_node SHARED plugins/action/reinitialize_global_localization_service.cpp)
|
||||
list(APPEND plugin_libs nav2_reinitialize_global_localization_service_bt_node)
|
||||
|
||||
add_library(nav2_rate_controller_bt_node SHARED plugins/decorator/rate_controller.cpp)
|
||||
list(APPEND plugin_libs nav2_rate_controller_bt_node)
|
||||
|
||||
add_library(nav2_distance_controller_bt_node SHARED plugins/decorator/distance_controller.cpp)
|
||||
list(APPEND plugin_libs nav2_distance_controller_bt_node)
|
||||
|
||||
add_library(nav2_speed_controller_bt_node SHARED plugins/decorator/speed_controller.cpp)
|
||||
list(APPEND plugin_libs nav2_speed_controller_bt_node)
|
||||
|
||||
add_library(nav2_truncate_path_action_bt_node SHARED plugins/action/truncate_path_action.cpp)
|
||||
list(APPEND plugin_libs nav2_truncate_path_action_bt_node)
|
||||
|
||||
add_library(nav2_truncate_path_local_action_bt_node SHARED plugins/action/truncate_path_local_action.cpp)
|
||||
list(APPEND plugin_libs nav2_truncate_path_local_action_bt_node)
|
||||
|
||||
add_library(nav2_goal_updater_node_bt_node SHARED plugins/decorator/goal_updater_node.cpp)
|
||||
list(APPEND plugin_libs nav2_goal_updater_node_bt_node)
|
||||
|
||||
add_library(nav2_path_longer_on_approach_bt_node SHARED plugins/decorator/path_longer_on_approach.cpp)
|
||||
list(APPEND plugin_libs nav2_path_longer_on_approach_bt_node)
|
||||
|
||||
add_library(nav2_recovery_node_bt_node SHARED plugins/control/recovery_node.cpp)
|
||||
list(APPEND plugin_libs nav2_recovery_node_bt_node)
|
||||
|
||||
add_library(nav2_navigate_to_pose_action_bt_node SHARED plugins/action/navigate_to_pose_action.cpp)
|
||||
list(APPEND plugin_libs nav2_navigate_to_pose_action_bt_node)
|
||||
|
||||
add_library(nav2_navigate_through_poses_action_bt_node SHARED plugins/action/navigate_through_poses_action.cpp)
|
||||
list(APPEND plugin_libs nav2_navigate_through_poses_action_bt_node)
|
||||
|
||||
add_library(nav2_remove_passed_goals_action_bt_node SHARED plugins/action/remove_passed_goals_action.cpp)
|
||||
list(APPEND plugin_libs nav2_remove_passed_goals_action_bt_node)
|
||||
|
||||
add_library(nav2_get_pose_from_path_action_bt_node SHARED plugins/action/get_pose_from_path_action.cpp)
|
||||
list(APPEND plugin_libs nav2_get_pose_from_path_action_bt_node)
|
||||
|
||||
add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp)
|
||||
list(APPEND plugin_libs nav2_pipeline_sequence_bt_node)
|
||||
|
||||
add_library(nav2_round_robin_node_bt_node SHARED plugins/control/round_robin_node.cpp)
|
||||
list(APPEND plugin_libs nav2_round_robin_node_bt_node)
|
||||
|
||||
add_library(nav2_single_trigger_bt_node SHARED plugins/decorator/single_trigger_node.cpp)
|
||||
list(APPEND plugin_libs nav2_single_trigger_bt_node)
|
||||
|
||||
add_library(nav2_planner_selector_bt_node SHARED plugins/action/planner_selector_node.cpp)
|
||||
list(APPEND plugin_libs nav2_planner_selector_bt_node)
|
||||
|
||||
add_library(nav2_controller_selector_bt_node SHARED plugins/action/controller_selector_node.cpp)
|
||||
list(APPEND plugin_libs nav2_controller_selector_bt_node)
|
||||
|
||||
add_library(nav2_smoother_selector_bt_node SHARED plugins/action/smoother_selector_node.cpp)
|
||||
list(APPEND plugin_libs nav2_smoother_selector_bt_node)
|
||||
|
||||
add_library(nav2_goal_checker_selector_bt_node SHARED plugins/action/goal_checker_selector_node.cpp)
|
||||
list(APPEND plugin_libs nav2_goal_checker_selector_bt_node)
|
||||
|
||||
add_library(nav2_progress_checker_selector_bt_node SHARED plugins/action/progress_checker_selector_node.cpp)
|
||||
list(APPEND plugin_libs nav2_progress_checker_selector_bt_node)
|
||||
|
||||
add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp)
|
||||
list(APPEND plugin_libs nav2_goal_updated_controller_bt_node)
|
||||
|
||||
foreach(bt_plugin ${plugin_libs})
|
||||
ament_target_dependencies(${bt_plugin} ${dependencies})
|
||||
target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
|
||||
endforeach()
|
||||
|
||||
install(TARGETS ${library_name}
|
||||
${plugin_libs}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include/
|
||||
)
|
||||
|
||||
install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME})
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
add_subdirectory(test)
|
||||
endif()
|
||||
|
||||
ament_export_include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
ament_export_libraries(
|
||||
${library_name}
|
||||
${plugin_libs}
|
||||
)
|
||||
|
||||
ament_export_dependencies(${dependencies})
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,66 @@
|
||||
# nav2_behavior_tree
|
||||
|
||||
This module is used by the nav2_bt_navigator to implement a ROS2 node that executes navigation Behavior Trees for either navigation or autonomy systems. The nav2_behavior_tree module uses the [Behavior-Tree.CPP library](https://github.com/BehaviorTree/BehaviorTree.CPP) for the core Behavior Tree processing.
|
||||
|
||||
The nav2_behavior_tree module provides:
|
||||
* A C++ template class for easily integrating ROS2 actions and services into Behavior Trees,
|
||||
* Navigation-specific behavior tree nodes, and
|
||||
* a generic BehaviorTreeEngine class that simplifies the integration of BT processing into ROS2 nodes for navigation or higher-level autonomy applications.
|
||||
|
||||
See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-bt-xml.html) for additional parameter descriptions and a list of XML nodes made available in this package. Also review the [Nav2 Behavior Tree Explanation](https://navigation.ros.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. A [tutorial](https://navigation.ros.org/plugin_tutorials/docs/writing_new_bt_plugin.html) is also provided to explain how to create a simple BT plugin.
|
||||
|
||||
See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins.
|
||||
|
||||
## The bt_action_node Template and the Behavior Tree Engine
|
||||
|
||||
The [bt_action_node template](include/nav2_behavior_tree/bt_action_node.hpp) allows one to easily integrate a ROS2 action into a BehaviorTree. To do so, one derives from the BtActionNode template, providing the action message type. For example,
|
||||
|
||||
```C++
|
||||
#include "nav2_msgs/action/follow_path.hpp"
|
||||
#include "nav2_behavior_tree/bt_action_node.hpp"
|
||||
|
||||
class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
|
||||
{
|
||||
...
|
||||
};
|
||||
```
|
||||
|
||||
The resulting node must be registered with the factory in the Behavior Tree engine in order to be available for use in Behavior Trees executed by this engine.
|
||||
|
||||
```C++
|
||||
BehaviorTreeEngine::BehaviorTreeEngine()
|
||||
{
|
||||
...
|
||||
|
||||
factory_.registerNodeType<nav2_behavior_tree::FollowPathAction>("FollowPath");
|
||||
|
||||
...
|
||||
}
|
||||
```
|
||||
|
||||
Once a new node is registered with the factory, it is now available to the BehaviorTreeEngine and can be used in Behavior Trees. For example, the following simple XML description of a BT shows the FollowPath node in use:
|
||||
|
||||
```XML
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<Sequence name="root">
|
||||
<ComputePathToPose goal="${goal}"/>
|
||||
<FollowPath path="${path}" controller_property="FollowPath"/>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
```
|
||||
The BehaviorTree engine has a run method that accepts an XML description of a BT for execution:
|
||||
|
||||
```C++
|
||||
BtStatus run(
|
||||
BT::Blackboard::Ptr & blackboard,
|
||||
const std::string & behavior_tree_xml,
|
||||
std::function<void()> onLoop,
|
||||
std::function<bool()> cancelRequested,
|
||||
std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10));
|
||||
```
|
||||
|
||||
See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine.
|
||||
|
||||
For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/3.8/learn-the-basics/BT_basics
|
||||
Binary file not shown.
@@ -0,0 +1,167 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
|
||||
<svg version="1.2" width="139.7mm" height="139.7mm" viewBox="0 0 13970 13970" preserveAspectRatio="xMidYMid" fill-rule="evenodd" stroke-width="28.222" stroke-linejoin="round" xmlns="http://www.w3.org/2000/svg" xmlns:ooo="http://xml.openoffice.org/svg/export" xmlns:xlink="http://www.w3.org/1999/xlink" xmlns:presentation="http://sun.com/xmlns/staroffice/presentation" xmlns:smil="http://www.w3.org/2001/SMIL20/" xmlns:anim="urn:oasis:names:tc:opendocument:xmlns:animation:1.0" xml:space="preserve">
|
||||
<defs class="ClipPathGroup">
|
||||
<clipPath id="presentation_clip_path" clipPathUnits="userSpaceOnUse">
|
||||
<rect x="0" y="0" width="13970" height="13970"/>
|
||||
</clipPath>
|
||||
</defs>
|
||||
<defs>
|
||||
<font id="EmbeddedFont_1" horiz-adv-x="2048">
|
||||
<font-face font-family="Verdana embedded" units-per-em="2048" font-weight="bold" font-style="normal" ascent="2037" descent="423"/>
|
||||
<missing-glyph horiz-adv-x="2048" d="M 0,0 L 2047,0 2047,2047 0,2047 0,0 Z"/>
|
||||
<glyph unicode="x" horiz-adv-x="1350" d="M 1344,0 L 924,0 684,336 438,0 26,0 469,561 33,1123 453,1123 689,793 926,1123 1339,1123 902,570 1344,0 Z"/>
|
||||
<glyph unicode="w" horiz-adv-x="1933" d="M 1961,1123 L 1613,0 1231,0 1002,757 777,0 390,0 45,1123 423,1123 615,349 855,1123 1174,1123 1402,349 1591,1123 1961,1123 Z"/>
|
||||
<glyph unicode="v" horiz-adv-x="1271" d="M 1301,1123 L 868,0 460,0 30,1123 411,1123 671,351 928,1123 1301,1123 Z"/>
|
||||
<glyph unicode="u" horiz-adv-x="1138" d="M 1289,0 L 929,0 929,124 C 862,73 801,35 746,9 691,-18 625,-31 548,-31 424,-31 329,5 262,76 195,147 161,253 161,392 L 161,1123 523,1123 523,566 C 523,509 525,462 529,425 532,387 541,356 554,331 567,306 586,288 612,277 637,266 673,260 719,260 750,260 784,266 821,277 858,288 894,305 929,327 L 929,1123 1289,1123 1289,0 Z"/>
|
||||
<glyph unicode="t" horiz-adv-x="875" d="M 888,10 C 849,0 808,-8 765,-14 722,-19 669,-22 606,-22 466,-22 362,6 294,63 225,120 191,217 191,354 L 191,879 43,879 43,1123 191,1123 191,1444 551,1444 551,1123 888,1123 888,879 551,879 551,481 C 551,442 551,407 552,378 553,349 558,322 568,299 577,276 594,257 618,244 641,230 676,223 721,223 740,223 764,227 795,235 825,243 846,250 858,257 L 888,257 888,10 Z"/>
|
||||
<glyph unicode="s" horiz-adv-x="1086" d="M 1146,356 C 1146,240 1094,147 990,76 886,5 744,-31 563,-31 463,-31 372,-22 291,-3 210,16 144,36 95,59 L 95,355 128,355 C 146,342 167,329 191,314 214,299 248,282 291,265 328,250 371,237 418,226 465,215 516,209 570,209 641,209 693,217 727,233 761,248 778,271 778,302 778,329 768,349 748,362 728,374 690,386 634,397 607,403 571,410 526,417 480,424 438,432 401,442 299,469 223,510 174,567 125,623 100,693 100,777 100,885 151,975 254,1047 356,1118 496,1154 673,1154 757,1154 838,1146 915,1129 992,1112 1052,1093 1095,1074 L 1095,790 1064,790 C 1011,827 950,857 882,880 813,903 744,914 674,914 616,914 567,906 528,890 488,874 468,851 468,822 468,795 477,775 495,761 513,747 555,733 622,719 659,712 698,705 741,698 783,691 825,682 868,671 963,646 1033,608 1078,555 1123,502 1146,435 1146,356 Z"/>
|
||||
<glyph unicode="p" horiz-adv-x="1191" d="M 1342,577 C 1342,484 1328,400 1300,324 1271,248 1234,185 1188,136 1140,84 1086,45 1025,18 964,-9 899,-22 830,-22 766,-22 712,-15 667,-2 622,12 577,31 530,56 L 530,-412 170,-412 170,1123 530,1123 530,1006 C 585,1050 642,1086 700,1113 757,1140 824,1154 899,1154 1039,1154 1148,1103 1226,1000 1303,897 1342,756 1342,577 Z M 971,570 C 971,679 952,759 915,808 878,857 819,882 738,882 704,882 669,877 633,867 597,856 563,841 530,822 L 530,257 C 554,248 580,242 607,239 634,236 661,235 688,235 783,235 854,263 901,319 948,374 971,458 971,570 Z"/>
|
||||
<glyph unicode="o" horiz-adv-x="1245" d="M 1318,561 C 1318,376 1264,231 1157,125 1049,18 898,-35 703,-35 508,-35 357,18 250,125 142,231 88,376 88,561 88,747 142,893 251,999 359,1105 510,1158 703,1158 899,1158 1051,1105 1158,998 1265,891 1318,746 1318,561 Z M 875,288 C 898,317 916,351 928,392 939,432 945,488 945,559 945,625 939,680 927,725 915,770 898,805 877,832 856,859 830,879 800,890 770,901 738,907 703,907 668,907 638,902 611,893 584,884 558,865 533,837 511,811 494,775 481,730 468,685 461,628 461,559 461,498 467,445 478,400 489,355 506,318 528,291 549,265 575,246 605,234 634,222 668,216 706,216 739,216 770,222 800,233 829,244 854,262 875,288 Z"/>
|
||||
<glyph unicode="n" horiz-adv-x="1138" d="M 1298,0 L 936,0 936,557 C 936,602 934,648 929,693 924,738 916,771 905,792 892,817 872,835 847,846 821,857 785,863 739,863 706,863 673,858 640,847 606,836 569,819 530,796 L 530,0 170,0 170,1123 530,1123 530,999 C 594,1049 656,1087 715,1114 774,1141 839,1154 911,1154 1032,1154 1127,1119 1196,1048 1264,977 1298,872 1298,731 L 1298,0 Z"/>
|
||||
<glyph unicode="m" horiz-adv-x="1853" d="M 1268,796 L 1268,0 906,0 906,561 C 906,616 905,662 902,701 899,739 892,770 880,794 868,818 850,836 826,847 801,858 767,863 723,863 688,863 653,856 620,842 587,827 557,812 530,796 L 530,0 170,0 170,1123 530,1123 530,999 C 592,1048 651,1086 708,1113 765,1140 827,1154 896,1154 970,1154 1035,1138 1092,1105 1149,1072 1193,1023 1225,959 1297,1020 1367,1067 1435,1102 1503,1137 1570,1154 1635,1154 1756,1154 1849,1118 1912,1045 1975,972 2006,868 2006,731 L 2006,0 1644,0 1644,561 C 1644,616 1643,663 1641,701 1638,739 1631,770 1619,794 1608,818 1590,836 1565,847 1540,858 1506,863 1461,863 1431,863 1402,858 1373,848 1344,837 1309,820 1268,796 Z"/>
|
||||
<glyph unicode="l" horiz-adv-x="371" d="M 530,0 L 170,0 170,1556 530,1556 530,0 Z"/>
|
||||
<glyph unicode="i" horiz-adv-x="398" d="M 530,0 L 170,0 170,1123 530,1123 530,0 Z M 540,1283 L 160,1283 160,1556 540,1556 540,1283 Z"/>
|
||||
<glyph unicode="h" horiz-adv-x="1138" d="M 1298,0 L 936,0 936,557 C 936,602 934,648 929,693 924,738 916,771 905,792 892,817 872,835 847,846 821,857 785,863 739,863 706,863 673,858 640,847 606,836 569,819 530,796 L 530,0 170,0 170,1556 530,1556 530,999 C 594,1049 656,1087 715,1114 774,1141 839,1154 911,1154 1032,1154 1127,1119 1196,1048 1264,977 1298,872 1298,731 L 1298,0 Z"/>
|
||||
<glyph unicode="g" horiz-adv-x="1192" d="M 1262,129 C 1262,24 1247,-65 1217,-137 1187,-209 1145,-265 1091,-306 1037,-347 972,-377 896,-396 819,-414 733,-423 637,-423 559,-423 482,-418 407,-409 331,-400 265,-388 210,-375 L 210,-94 254,-94 C 298,-111 352,-127 415,-142 478,-156 535,-163 585,-163 652,-163 706,-157 748,-145 789,-132 821,-115 843,-92 864,-71 879,-43 888,-10 897,23 902,63 902,110 L 902,131 C 859,96 811,68 758,47 705,26 647,16 582,16 425,16 303,63 218,158 133,253 90,397 90,590 90,683 103,763 129,830 155,897 192,956 239,1006 283,1053 337,1089 402,1115 466,1141 532,1154 599,1154 660,1154 715,1147 765,1133 814,1118 859,1098 900,1073 L 913,1123 1262,1123 1262,129 Z M 902,354 L 902,863 C 881,872 856,879 826,884 796,889 769,891 745,891 650,891 579,864 532,810 485,755 461,679 461,582 461,474 481,399 522,356 562,313 622,292 701,292 737,292 772,298 807,309 842,320 873,335 902,354 Z"/>
|
||||
<glyph unicode="e" horiz-adv-x="1192" d="M 1276,495 L 452,495 C 457,407 491,340 553,293 614,246 705,223 825,223 901,223 975,237 1046,264 1117,291 1174,321 1215,352 L 1255,352 1255,63 C 1174,30 1097,7 1025,-8 953,-23 873,-30 786,-30 561,-30 388,21 268,122 148,223 88,368 88,555 88,740 145,887 259,996 372,1104 528,1158 726,1158 909,1158 1046,1112 1138,1020 1230,927 1276,794 1276,621 L 1276,495 Z M 918,706 C 916,781 897,838 862,876 827,914 772,933 697,933 628,933 571,915 526,879 481,843 456,785 451,706 L 918,706 Z"/>
|
||||
<glyph unicode="c" horiz-adv-x="1060" d="M 755,-30 C 657,-30 568,-18 487,5 406,28 335,64 275,113 216,162 170,223 137,297 104,371 88,458 88,557 88,662 106,752 141,828 176,904 224,967 287,1017 348,1064 418,1099 497,1121 576,1143 659,1154 744,1154 821,1154 891,1146 956,1129 1021,1112 1081,1091 1137,1064 L 1137,757 1086,757 C 1072,769 1055,783 1036,799 1016,815 992,831 963,846 936,861 906,873 873,883 840,892 802,897 759,897 663,897 589,867 538,806 486,745 460,662 460,557 460,449 487,367 540,311 593,255 668,227 765,227 810,227 851,232 888,243 924,253 954,265 978,279 1001,292 1021,306 1038,321 1055,336 1071,350 1086,364 L 1137,364 1137,57 C 1080,30 1021,9 960,-7 898,-22 830,-30 755,-30 Z"/>
|
||||
<glyph unicode="a" horiz-adv-x="1139" d="M 850,293 L 850,527 C 801,523 749,518 692,511 635,504 592,495 563,486 527,475 500,458 481,437 462,415 452,386 452,351 452,328 454,309 458,294 462,279 472,265 488,252 503,239 522,229 543,223 564,216 598,213 643,213 679,213 716,220 753,235 790,250 822,269 850,293 Z M 850,119 C 831,104 807,87 778,66 749,45 722,29 697,17 662,1 625,-11 587,-19 549,-26 507,-30 462,-30 355,-30 266,3 194,70 122,136 86,221 86,324 86,406 104,474 141,526 178,578 230,619 297,650 364,680 446,701 545,714 644,727 746,733 852,733 L 852,739 C 852,803 827,847 776,871 725,896 651,908 552,908 493,908 429,898 362,877 295,856 246,839 217,828 L 184,828 184,1099 C 222,1109 284,1121 370,1135 455,1148 541,1155 627,1155 832,1155 980,1124 1071,1061 1162,998 1207,899 1207,764 L 1207,0 850,0 850,119 Z"/>
|
||||
<glyph unicode="T" horiz-adv-x="1324" d="M 1355,1201 L 890,1201 890,0 506,0 506,1201 41,1201 41,1489 1355,1489 1355,1201 Z"/>
|
||||
<glyph unicode="P" horiz-adv-x="1245" d="M 1419,1019 C 1419,952 1407,887 1384,824 1361,760 1327,706 1284,663 1225,604 1159,560 1086,530 1013,500 922,485 813,485 L 574,485 574,0 190,0 190,1489 822,1489 C 917,1489 997,1481 1062,1465 1127,1448 1184,1424 1234,1391 1294,1352 1340,1301 1372,1240 1403,1179 1419,1105 1419,1019 Z M 1022,1011 C 1022,1053 1011,1090 988,1120 965,1151 939,1172 909,1184 869,1200 830,1208 792,1208 754,1208 703,1208 640,1208 L 574,1208 574,765 684,765 C 749,765 803,769 846,777 888,785 923,801 952,825 977,847 995,872 1006,902 1017,932 1022,968 1022,1011 Z"/>
|
||||
<glyph unicode="N" horiz-adv-x="1377" d="M 1544,0 L 1174,0 542,1022 542,0 190,0 190,1489 649,1489 1192,636 1192,1489 1544,1489 1544,0 Z"/>
|
||||
<glyph unicode="M" horiz-adv-x="1562" d="M 1751,0 L 1369,0 1369,997 1093,350 828,350 552,997 552,0 190,0 190,1489 636,1489 971,742 1305,1489 1751,1489 1751,0 Z"/>
|
||||
<glyph unicode="F" horiz-adv-x="1086" d="M 1257,1201 L 572,1201 572,924 1207,924 1207,636 572,636 572,0 190,0 190,1489 1257,1489 1257,1201 Z"/>
|
||||
<glyph unicode="E" horiz-adv-x="1086" d="M 1267,0 L 190,0 190,1489 1267,1489 1267,1201 572,1201 572,944 1217,944 1217,656 572,656 572,288 1267,288 1267,0 Z"/>
|
||||
<glyph unicode="C" horiz-adv-x="1324" d="M 863,-29 C 752,-29 650,-13 557,20 463,53 382,101 315,166 248,231 196,311 159,408 122,505 103,616 103,743 103,861 121,968 156,1064 191,1160 243,1242 310,1311 375,1377 455,1428 551,1464 646,1500 751,1518 864,1518 927,1518 983,1515 1034,1508 1084,1501 1130,1491 1173,1480 1218,1467 1258,1453 1295,1438 1331,1422 1363,1407 1390,1394 L 1390,1033 1346,1033 C 1327,1049 1304,1068 1276,1090 1247,1112 1215,1134 1179,1155 1142,1176 1103,1194 1060,1209 1017,1224 972,1231 923,1231 869,1231 818,1223 769,1206 720,1189 675,1160 634,1121 595,1083 563,1033 539,970 514,907 502,831 502,742 502,649 515,571 542,508 568,445 601,396 641,360 682,323 727,297 777,282 827,266 876,258 925,258 972,258 1018,265 1064,279 1109,293 1151,312 1190,336 1223,355 1253,376 1281,398 1309,420 1332,439 1350,455 L 1390,455 1390,99 C 1353,82 1317,67 1283,52 1249,37 1213,25 1176,14 1127,0 1082,-11 1039,-18 996,-25 938,-29 863,-29 Z"/>
|
||||
</font>
|
||||
</defs>
|
||||
<defs class="TextShapeIndex">
|
||||
<g ooo:slide="id1" ooo:id-list="id3 id4 id5 id6 id7 id8 id9"/>
|
||||
</defs>
|
||||
<defs class="EmbeddedBulletChars">
|
||||
<g id="bullet-char-template(57356)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M 580,1141 L 1163,571 580,0 -4,571 580,1141 Z"/>
|
||||
</g>
|
||||
<g id="bullet-char-template(57354)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M 8,1128 L 1137,1128 1137,0 8,0 8,1128 Z"/>
|
||||
</g>
|
||||
<g id="bullet-char-template(10146)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M 174,0 L 602,739 174,1481 1456,739 174,0 Z M 1358,739 L 309,1346 659,739 1358,739 Z"/>
|
||||
</g>
|
||||
<g id="bullet-char-template(10132)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M 2015,739 L 1276,0 717,0 1260,543 174,543 174,936 1260,936 717,1481 1274,1481 2015,739 Z"/>
|
||||
</g>
|
||||
<g id="bullet-char-template(10007)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M 0,-2 C -7,14 -16,27 -25,37 L 356,567 C 262,823 215,952 215,954 215,979 228,992 255,992 264,992 276,990 289,987 310,991 331,999 354,1012 L 381,999 492,748 772,1049 836,1024 860,1049 C 881,1039 901,1025 922,1006 886,937 835,863 770,784 769,783 710,716 594,584 L 774,223 C 774,196 753,168 711,139 L 727,119 C 717,90 699,76 672,76 641,76 570,178 457,381 L 164,-76 C 142,-110 111,-127 72,-127 30,-127 9,-110 8,-76 1,-67 -2,-52 -2,-32 -2,-23 -1,-13 0,-2 Z"/>
|
||||
</g>
|
||||
<g id="bullet-char-template(10004)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M 285,-33 C 182,-33 111,30 74,156 52,228 41,333 41,471 41,549 55,616 82,672 116,743 169,778 240,778 293,778 328,747 346,684 L 369,508 C 377,444 397,411 428,410 L 1163,1116 C 1174,1127 1196,1133 1229,1133 1271,1133 1292,1118 1292,1087 L 1292,965 C 1292,929 1282,901 1262,881 L 442,47 C 390,-6 338,-33 285,-33 Z"/>
|
||||
</g>
|
||||
<g id="bullet-char-template(9679)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M 813,0 C 632,0 489,54 383,161 276,268 223,411 223,592 223,773 276,916 383,1023 489,1130 632,1184 813,1184 992,1184 1136,1130 1245,1023 1353,916 1407,772 1407,592 1407,412 1353,268 1245,161 1136,54 992,0 813,0 Z"/>
|
||||
</g>
|
||||
<g id="bullet-char-template(8226)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M 346,457 C 273,457 209,483 155,535 101,586 74,649 74,723 74,796 101,859 155,911 209,963 273,989 346,989 419,989 480,963 531,910 582,859 608,796 608,723 608,648 583,586 532,535 482,483 420,457 346,457 Z"/>
|
||||
</g>
|
||||
<g id="bullet-char-template(8211)" transform="scale(0.00048828125,-0.00048828125)">
|
||||
<path d="M -4,459 L 1135,459 1135,606 -4,606 -4,459 Z"/>
|
||||
</g>
|
||||
</defs>
|
||||
<defs class="TextEmbeddedBitmaps"/>
|
||||
<g>
|
||||
<g id="id2" class="Master_Slide">
|
||||
<g id="bg-id2" class="Background"/>
|
||||
<g id="bo-id2" class="BackgroundObjects"/>
|
||||
</g>
|
||||
</g>
|
||||
<g class="SlideGroup">
|
||||
<g>
|
||||
<g id="id1" class="Slide" clip-path="url(#presentation_clip_path)">
|
||||
<g class="Page">
|
||||
<g class="com.sun.star.drawing.CustomShape">
|
||||
<g id="id3">
|
||||
<rect class="BoundingBox" stroke="none" fill="none" x="4528" y="1533" width="4776" height="2363"/>
|
||||
<path fill="rgb(128,128,128)" stroke="none" d="M 5089,1734 C 4909,1734 4729,1914 4729,2094 L 4729,3534 C 4729,3714 4909,3894 5089,3894 L 8942,3894 C 9122,3894 9302,3714 9302,3534 L 9302,2094 C 9302,1914 9122,1734 8942,1734 L 5089,1734 Z M 4729,1734 L 4729,1734 Z M 9302,3894 L 9302,3894 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 5089,1734 C 4909,1734 4729,1914 4729,2094 L 4729,3534 C 4729,3714 4909,3894 5089,3894 L 8942,3894 C 9122,3894 9302,3714 9302,3534 L 9302,2094 C 9302,1914 9122,1734 8942,1734 L 5089,1734 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 4729,1734 L 4729,1734 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 9302,3894 L 9302,3894 Z"/>
|
||||
<text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="5186" y="2983"><tspan fill="rgb(128,128,128)" stroke="none">ExecuteMission</tspan></tspan></tspan></text>
|
||||
<path fill="rgb(0,102,204)" stroke="none" d="M 4889,1534 C 4709,1534 4529,1714 4529,1894 L 4529,3334 C 4529,3514 4709,3694 4889,3694 L 8742,3694 C 8922,3694 9102,3514 9102,3334 L 9102,1894 C 9102,1714 8922,1534 8742,1534 L 4889,1534 Z M 4529,1534 L 4529,1534 Z M 9102,3694 L 9102,3694 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 4889,1534 C 4709,1534 4529,1714 4529,1894 L 4529,3334 C 4529,3514 4709,3694 4889,3694 L 8742,3694 C 8922,3694 9102,3514 9102,3334 L 9102,1894 C 9102,1714 8922,1534 8742,1534 L 4889,1534 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 4529,1534 L 4529,1534 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 9102,3694 L 9102,3694 Z"/>
|
||||
<text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="4986" y="2783"><tspan fill="rgb(255,255,255)" stroke="none">ExecuteMission</tspan></tspan></tspan></text>
|
||||
</g>
|
||||
</g>
|
||||
<g class="com.sun.star.drawing.CustomShape">
|
||||
<g id="id4">
|
||||
<rect class="BoundingBox" stroke="none" fill="none" x="4655" y="5470" width="4776" height="2363"/>
|
||||
<path fill="rgb(128,128,128)" stroke="none" d="M 5216,5671 C 5036,5671 4856,5851 4856,6031 L 4856,7471 C 4856,7651 5036,7831 5216,7831 L 9069,7831 C 9249,7831 9429,7651 9429,7471 L 9429,6031 C 9429,5851 9249,5671 9069,5671 L 5216,5671 Z M 4856,5671 L 4856,5671 Z M 9429,7831 L 9429,7831 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 5216,5671 C 5036,5671 4856,5851 4856,6031 L 4856,7471 C 4856,7651 5036,7831 5216,7831 L 9069,7831 C 9249,7831 9429,7651 9429,7471 L 9429,6031 C 9429,5851 9249,5671 9069,5671 L 5216,5671 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 4856,5671 L 4856,5671 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 9429,7831 L 9429,7831 Z"/>
|
||||
<text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="5229" y="6920"><tspan fill="rgb(128,128,128)" stroke="none">NavigateToPose</tspan></tspan></tspan></text>
|
||||
<path fill="rgb(0,102,204)" stroke="none" d="M 5016,5471 C 4836,5471 4656,5651 4656,5831 L 4656,7271 C 4656,7451 4836,7631 5016,7631 L 8869,7631 C 9049,7631 9229,7451 9229,7271 L 9229,5831 C 9229,5651 9049,5471 8869,5471 L 5016,5471 Z M 4656,5471 L 4656,5471 Z M 9229,7631 L 9229,7631 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 5016,5471 C 4836,5471 4656,5651 4656,5831 L 4656,7271 C 4656,7451 4836,7631 5016,7631 L 8869,7631 C 9049,7631 9229,7451 9229,7271 L 9229,5831 C 9229,5651 9049,5471 8869,5471 L 5016,5471 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 4656,5471 L 4656,5471 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 9229,7631 L 9229,7631 Z"/>
|
||||
<text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="5029" y="6720"><tspan fill="rgb(255,255,255)" stroke="none">NavigateToPose</tspan></tspan></tspan></text>
|
||||
</g>
|
||||
</g>
|
||||
<g class="com.sun.star.drawing.CustomShape">
|
||||
<g id="id5">
|
||||
<rect class="BoundingBox" stroke="none" fill="none" x="1352" y="9788" width="5665" height="2363"/>
|
||||
<path fill="rgb(128,128,128)" stroke="none" d="M 1912,9989 C 1732,9989 1553,10169 1553,10349 L 1553,11789 C 1553,11969 1732,12149 1912,12149 L 6655,12149 C 6835,12149 7015,11969 7015,11789 L 7015,10349 C 7015,10169 6835,9989 6655,9989 L 1912,9989 Z M 1553,9989 L 1553,9989 Z M 7015,12149 L 7015,12149 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 1912,9989 C 1732,9989 1553,10169 1553,10349 L 1553,11789 C 1553,11969 1732,12149 1912,12149 L 6655,12149 C 6835,12149 7015,11969 7015,11789 L 7015,10349 C 7015,10169 6835,9989 6655,9989 L 1912,9989 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 1553,9989 L 1553,9989 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 7015,12149 L 7015,12149 Z"/>
|
||||
<text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="1826" y="11238"><tspan fill="rgb(128,128,128)" stroke="none">ComputePathToPose</tspan></tspan></tspan></text>
|
||||
<path fill="rgb(0,102,204)" stroke="none" d="M 1712,9789 C 1532,9789 1353,9969 1353,10149 L 1353,11589 C 1353,11769 1532,11949 1712,11949 L 6455,11949 C 6635,11949 6815,11769 6815,11589 L 6815,10149 C 6815,9969 6635,9789 6455,9789 L 1712,9789 Z M 1353,9789 L 1353,9789 Z M 6815,11949 L 6815,11949 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 1712,9789 C 1532,9789 1353,9969 1353,10149 L 1353,11589 C 1353,11769 1532,11949 1712,11949 L 6455,11949 C 6635,11949 6815,11769 6815,11589 L 6815,10149 C 6815,9969 6635,9789 6455,9789 L 1712,9789 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 1353,9789 L 1353,9789 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 6815,11949 L 6815,11949 Z"/>
|
||||
<text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="1626" y="11038"><tspan fill="rgb(255,255,255)" stroke="none">ComputePathToPose</tspan></tspan></tspan></text>
|
||||
</g>
|
||||
</g>
|
||||
<g class="com.sun.star.drawing.CustomShape">
|
||||
<g id="id6">
|
||||
<rect class="BoundingBox" stroke="none" fill="none" x="7703" y="9788" width="4776" height="2363"/>
|
||||
<path fill="rgb(128,128,128)" stroke="none" d="M 8264,9989 C 8084,9989 7904,10169 7904,10349 L 7904,11789 C 7904,11969 8084,12149 8264,12149 L 12117,12149 C 12297,12149 12477,11969 12477,11789 L 12477,10349 C 12477,10169 12297,9989 12117,9989 L 8264,9989 Z M 7904,9989 L 7904,9989 Z M 12477,12149 L 12477,12149 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 8264,9989 C 8084,9989 7904,10169 7904,10349 L 7904,11789 C 7904,11969 8084,12149 8264,12149 L 12117,12149 C 12297,12149 12477,11969 12477,11789 L 12477,10349 C 12477,10169 12297,9989 12117,9989 L 8264,9989 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 7904,9989 L 7904,9989 Z"/>
|
||||
<path fill="none" stroke="rgb(128,128,128)" d="M 12477,12149 L 12477,12149 Z"/>
|
||||
<text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="8865" y="11238"><tspan fill="rgb(128,128,128)" stroke="none">FollowPath</tspan></tspan></tspan></text>
|
||||
<path fill="rgb(0,102,204)" stroke="none" d="M 8064,9789 C 7884,9789 7704,9969 7704,10149 L 7704,11589 C 7704,11769 7884,11949 8064,11949 L 11917,11949 C 12097,11949 12277,11769 12277,11589 L 12277,10149 C 12277,9969 12097,9789 11917,9789 L 8064,9789 Z M 7704,9789 L 7704,9789 Z M 12277,11949 L 12277,11949 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 8064,9789 C 7884,9789 7704,9969 7704,10149 L 7704,11589 C 7704,11769 7884,11949 8064,11949 L 11917,11949 C 12097,11949 12277,11769 12277,11589 L 12277,10149 C 12277,9969 12097,9789 11917,9789 L 8064,9789 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 7704,9789 L 7704,9789 Z"/>
|
||||
<path fill="none" stroke="rgb(52,101,164)" d="M 12277,11949 L 12277,11949 Z"/>
|
||||
<text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="8665" y="11038"><tspan fill="rgb(255,255,255)" stroke="none">FollowPath</tspan></tspan></tspan></text>
|
||||
</g>
|
||||
</g>
|
||||
<g class="com.sun.star.drawing.LineShape">
|
||||
<g id="id7">
|
||||
<rect class="BoundingBox" stroke="none" fill="none" x="6792" y="3693" width="301" height="1780"/>
|
||||
<path fill="none" stroke="rgb(0,0,0)" d="M 6942,3694 L 6942,5042"/>
|
||||
<path fill="rgb(0,0,0)" stroke="none" d="M 6942,5472 L 7092,5022 6792,5022 6942,5472 Z"/>
|
||||
</g>
|
||||
</g>
|
||||
<g class="com.sun.star.drawing.LineShape">
|
||||
<g id="id8">
|
||||
<rect class="BoundingBox" stroke="none" fill="none" x="6814" y="7630" width="3050" height="2161"/>
|
||||
<path fill="none" stroke="rgb(0,0,0)" d="M 6815,7631 L 9512,9541"/>
|
||||
<path fill="rgb(0,0,0)" stroke="none" d="M 9863,9790 L 9582,9407 9409,9652 9863,9790 Z"/>
|
||||
</g>
|
||||
</g>
|
||||
<g class="com.sun.star.drawing.LineShape">
|
||||
<g id="id9">
|
||||
<rect class="BoundingBox" stroke="none" fill="none" x="4021" y="7630" width="2796" height="2161"/>
|
||||
<path fill="none" stroke="rgb(0,0,0)" d="M 6815,7631 L 4361,9527"/>
|
||||
<path fill="rgb(0,0,0)" stroke="none" d="M 4021,9790 L 4469,9634 4285,9396 4021,9790 Z"/>
|
||||
</g>
|
||||
</g>
|
||||
</g>
|
||||
</g>
|
||||
</g>
|
||||
</g>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 24 KiB |
@@ -0,0 +1,12 @@
|
||||
# Instructions on using Groot
|
||||
[Groot](https://github.com/BehaviorTree/Groot) is the companion application of [BehaviorTree.CPP](https://github.com/BehaviorTree/BehaviorTree.CPP) to create, edit, and monitor behavior trees.
|
||||
|
||||
##### Note: Currently fully supports visualization of the behavior trees. It also supports the creation of custom nodes except control flow nodes. Support for custom control flow nodes and real-time monitoring is under development.
|
||||
|
||||
### BehaviorTree visualization
|
||||
To visualize the behavior trees using Groot:
|
||||
1. Open Groot in editor mode
|
||||
2. Select the `Load palette from file` option (import button) near the top left corner.
|
||||
3. Open the file `/path/to/nav2/nav2_behavior_tree/nav2_tree_nodes.xml` to import all the behavior tree nodes used for navigation.
|
||||
4. Select `Load tree` option near the top left corner
|
||||
5. Browse the tree you want to visualize the select ok.
|
||||
@@ -0,0 +1,101 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
// Copyright (c) 2020 Florian Gramss
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__BEHAVIOR_TREE_ENGINE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__BEHAVIOR_TREE_ENGINE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "behaviortree_cpp_v3/behavior_tree.h"
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
#include "behaviortree_cpp_v3/xml_parsing.h"
|
||||
#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @enum nav2_behavior_tree::BtStatus
|
||||
* @brief An enum class representing BT execution status
|
||||
*/
|
||||
enum class BtStatus { SUCCEEDED, FAILED, CANCELED };
|
||||
|
||||
/**
|
||||
* @class nav2_behavior_tree::BehaviorTreeEngine
|
||||
* @brief A class to create and handle behavior trees
|
||||
*/
|
||||
class BehaviorTreeEngine
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::BehaviorTreeEngine
|
||||
* @param plugin_libraries vector of BT plugin library names to load
|
||||
*/
|
||||
explicit BehaviorTreeEngine(
|
||||
const std::vector<std::string> & plugin_libraries);
|
||||
virtual ~BehaviorTreeEngine() {}
|
||||
|
||||
/**
|
||||
* @brief Function to execute a BT at a specific rate
|
||||
* @param tree BT to execute
|
||||
* @param onLoop Function to execute on each iteration of BT execution
|
||||
* @param cancelRequested Function to check if cancel was requested during BT execution
|
||||
* @param loopTimeout Time period for each iteration of BT execution
|
||||
* @return nav2_behavior_tree::BtStatus Status of BT execution
|
||||
*/
|
||||
BtStatus run(
|
||||
BT::Tree * tree,
|
||||
std::function<void()> onLoop,
|
||||
std::function<bool()> cancelRequested,
|
||||
std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10));
|
||||
|
||||
/**
|
||||
* @brief Function to create a BT from a XML string
|
||||
* @param xml_string XML string representing BT
|
||||
* @param blackboard Blackboard for BT
|
||||
* @return BT::Tree Created behavior tree
|
||||
*/
|
||||
BT::Tree createTreeFromText(
|
||||
const std::string & xml_string,
|
||||
BT::Blackboard::Ptr blackboard);
|
||||
|
||||
/**
|
||||
* @brief Function to create a BT from an XML file
|
||||
* @param file_path Path to BT XML file
|
||||
* @param blackboard Blackboard for BT
|
||||
* @return BT::Tree Created behavior tree
|
||||
*/
|
||||
BT::Tree createTreeFromFile(
|
||||
const std::string & file_path,
|
||||
BT::Blackboard::Ptr blackboard);
|
||||
|
||||
/**
|
||||
* @brief Function to explicitly reset all BT nodes to initial state
|
||||
* @param root_node Pointer to BT root node
|
||||
*/
|
||||
void haltAllActions(BT::TreeNode * root_node);
|
||||
|
||||
protected:
|
||||
// The factory that will be used to dynamically construct the behavior tree
|
||||
BT::BehaviorTreeFactory factory_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__BEHAVIOR_TREE_ENGINE_HPP_
|
||||
@@ -0,0 +1,482 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <chrono>
|
||||
|
||||
#include "behaviortree_cpp_v3/action_node.h"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "nav2_behavior_tree/bt_conversions.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
using namespace std::chrono_literals; // NOLINT
|
||||
|
||||
/**
|
||||
* @brief Abstract class representing an action based BT node
|
||||
* @tparam ActionT Type of action
|
||||
*/
|
||||
template<class ActionT>
|
||||
class BtActionNode : public BT::ActionNodeBase
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode constructor
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
BtActionNode(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name), should_send_goal_(true)
|
||||
{
|
||||
node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
|
||||
callback_group_ = node_->create_callback_group(
|
||||
rclcpp::CallbackGroupType::MutuallyExclusive,
|
||||
false);
|
||||
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
|
||||
|
||||
// Get the required items from the blackboard
|
||||
auto bt_loop_duration =
|
||||
config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
|
||||
server_timeout_ =
|
||||
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
|
||||
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
|
||||
wait_for_service_timeout_ =
|
||||
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
|
||||
|
||||
// timeout should be less than bt_loop_duration to be able to finish the current tick
|
||||
max_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(bt_loop_duration * 0.5);
|
||||
|
||||
// Initialize the input and output messages
|
||||
goal_ = typename ActionT::Goal();
|
||||
result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
|
||||
|
||||
std::string remapped_action_name;
|
||||
if (getInput("server_name", remapped_action_name)) {
|
||||
action_name_ = remapped_action_name;
|
||||
}
|
||||
createActionClient(action_name_);
|
||||
|
||||
// Give the derive class a chance to do any initialization
|
||||
RCLCPP_DEBUG(node_->get_logger(), "\"%s\" BtActionNode initialized", xml_tag_name.c_str());
|
||||
}
|
||||
|
||||
BtActionNode() = delete;
|
||||
|
||||
virtual ~BtActionNode()
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Create instance of an action client
|
||||
* @param action_name Action name to create client for
|
||||
*/
|
||||
void createActionClient(const std::string & action_name)
|
||||
{
|
||||
// Now that we have the ROS node to use, create the action client for this BT action
|
||||
action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name, callback_group_);
|
||||
|
||||
// Make sure the server is actually there before continuing
|
||||
RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
|
||||
if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
|
||||
RCLCPP_ERROR(
|
||||
node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs",
|
||||
action_name.c_str(),
|
||||
wait_for_service_timeout_.count() / 1000.0);
|
||||
throw std::runtime_error(
|
||||
std::string("Action server ") + action_name +
|
||||
std::string(" not available"));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Any subclass of BtActionNode that accepts parameters must provide a
|
||||
* providedPorts method and call providedBasicPorts in it.
|
||||
* @param addition Additional ports to add to BT port list
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedBasicPorts(BT::PortsList addition)
|
||||
{
|
||||
BT::PortsList basic = {
|
||||
BT::InputPort<std::string>("server_name", "Action server name"),
|
||||
BT::InputPort<std::chrono::milliseconds>("server_timeout")
|
||||
};
|
||||
basic.insert(addition.begin(), addition.end());
|
||||
|
||||
return basic;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts({});
|
||||
}
|
||||
|
||||
// Derived classes can override any of the following methods to hook into the
|
||||
// processing for the action: on_tick, on_wait_for_result, and on_success
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation on tick
|
||||
* Could do dynamic checks, such as getting updates to values on the blackboard
|
||||
*/
|
||||
virtual void on_tick()
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation after a timeout
|
||||
* waiting for a result that hasn't been received yet. Also provides access to
|
||||
* the latest feedback message from the action server. Feedback will be nullptr
|
||||
* in subsequent calls to this function if no new feedback is received while waiting for a result.
|
||||
* @param feedback shared_ptr to latest feedback message, nullptr if no new feedback was received
|
||||
*/
|
||||
virtual void on_wait_for_result(std::shared_ptr<const typename ActionT::Feedback>/*feedback*/)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation upon successful
|
||||
* completion of the action. Could put a value on the blackboard.
|
||||
* @return BT::NodeStatus Returns SUCCESS by default, user may override return another value
|
||||
*/
|
||||
virtual BT::NodeStatus on_success()
|
||||
{
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation when the action is aborted.
|
||||
* @return BT::NodeStatus Returns FAILURE by default, user may override return another value
|
||||
*/
|
||||
virtual BT::NodeStatus on_aborted()
|
||||
{
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation when the action is cancelled.
|
||||
* @return BT::NodeStatus Returns SUCCESS by default, user may override return another value
|
||||
*/
|
||||
virtual BT::NodeStatus on_cancelled()
|
||||
{
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override
|
||||
{
|
||||
// first step to be done only at the beginning of the Action
|
||||
if (status() == BT::NodeStatus::IDLE) {
|
||||
// setting the status to RUNNING to notify the BT Loggers (if any)
|
||||
setStatus(BT::NodeStatus::RUNNING);
|
||||
|
||||
// reset the flag to send the goal or not, allowing the user the option to set it in on_tick
|
||||
should_send_goal_ = true;
|
||||
|
||||
// user defined callback, may modify "should_send_goal_".
|
||||
on_tick();
|
||||
|
||||
if (!should_send_goal_) {
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
send_new_goal();
|
||||
}
|
||||
|
||||
try {
|
||||
// if new goal was sent and action server has not yet responded
|
||||
// check the future goal handle
|
||||
if (future_goal_handle_) {
|
||||
auto elapsed = (node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>();
|
||||
if (!is_future_goal_handle_complete(elapsed)) {
|
||||
// return RUNNING if there is still some time before timeout happens
|
||||
if (elapsed < server_timeout_) {
|
||||
return BT::NodeStatus::RUNNING;
|
||||
}
|
||||
// if server has taken more time than the specified timeout value return FAILURE
|
||||
RCLCPP_WARN(
|
||||
node_->get_logger(),
|
||||
"Timed out while waiting for action server to acknowledge goal request for %s",
|
||||
action_name_.c_str());
|
||||
future_goal_handle_.reset();
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
}
|
||||
|
||||
// The following code corresponds to the "RUNNING" loop
|
||||
if (rclcpp::ok() && !goal_result_available_) {
|
||||
// user defined callback. May modify the value of "goal_updated_"
|
||||
on_wait_for_result(feedback_);
|
||||
|
||||
// reset feedback to avoid stale information
|
||||
feedback_.reset();
|
||||
|
||||
auto goal_status = goal_handle_->get_status();
|
||||
if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
|
||||
goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED))
|
||||
{
|
||||
goal_updated_ = false;
|
||||
send_new_goal();
|
||||
auto elapsed = (node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>();
|
||||
if (!is_future_goal_handle_complete(elapsed)) {
|
||||
if (elapsed < server_timeout_) {
|
||||
return BT::NodeStatus::RUNNING;
|
||||
}
|
||||
RCLCPP_WARN(
|
||||
node_->get_logger(),
|
||||
"Timed out while waiting for action server to acknowledge goal request for %s",
|
||||
action_name_.c_str());
|
||||
future_goal_handle_.reset();
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
}
|
||||
|
||||
callback_group_executor_.spin_some();
|
||||
|
||||
// check if, after invoking spin_some(), we finally received the result
|
||||
if (!goal_result_available_) {
|
||||
// Yield this Action, returning RUNNING
|
||||
return BT::NodeStatus::RUNNING;
|
||||
}
|
||||
}
|
||||
} catch (const std::runtime_error & e) {
|
||||
if (e.what() == std::string("send_goal failed") ||
|
||||
e.what() == std::string("Goal was rejected by the action server"))
|
||||
{
|
||||
// Action related failure that should not fail the tree, but the node
|
||||
return BT::NodeStatus::FAILURE;
|
||||
} else {
|
||||
// Internal exception to propagate to the tree
|
||||
throw e;
|
||||
}
|
||||
}
|
||||
|
||||
BT::NodeStatus status;
|
||||
switch (result_.code) {
|
||||
case rclcpp_action::ResultCode::SUCCEEDED:
|
||||
status = on_success();
|
||||
break;
|
||||
|
||||
case rclcpp_action::ResultCode::ABORTED:
|
||||
status = on_aborted();
|
||||
break;
|
||||
|
||||
case rclcpp_action::ResultCode::CANCELED:
|
||||
status = on_cancelled();
|
||||
break;
|
||||
|
||||
default:
|
||||
throw std::logic_error("BtActionNode::Tick: invalid status value");
|
||||
}
|
||||
|
||||
goal_handle_.reset();
|
||||
return status;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief The other (optional) override required by a BT action. In this case, we
|
||||
* make sure to cancel the ROS2 action if it is still running.
|
||||
*/
|
||||
void halt() override
|
||||
{
|
||||
if (should_cancel_goal()) {
|
||||
auto future_result = action_client_->async_get_result(goal_handle_);
|
||||
auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
|
||||
if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Failed to cancel action server for %s", action_name_.c_str());
|
||||
}
|
||||
|
||||
if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Failed to get result for %s in node halt!", action_name_.c_str());
|
||||
}
|
||||
|
||||
on_cancelled();
|
||||
}
|
||||
|
||||
setStatus(BT::NodeStatus::IDLE);
|
||||
}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Function to check if current goal should be cancelled
|
||||
* @return bool True if current goal should be cancelled, false otherwise
|
||||
*/
|
||||
bool should_cancel_goal()
|
||||
{
|
||||
// Shut the node down if it is currently running
|
||||
if (status() != BT::NodeStatus::RUNNING) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// No need to cancel the goal if goal handle is invalid
|
||||
if (!goal_handle_) {
|
||||
return false;
|
||||
}
|
||||
|
||||
callback_group_executor_.spin_some();
|
||||
auto status = goal_handle_->get_status();
|
||||
|
||||
// Check if the goal is still executing
|
||||
return status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
|
||||
status == action_msgs::msg::GoalStatus::STATUS_EXECUTING;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Function to send new goal to action server
|
||||
*/
|
||||
void send_new_goal()
|
||||
{
|
||||
goal_result_available_ = false;
|
||||
auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions();
|
||||
send_goal_options.result_callback =
|
||||
[this](const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result) {
|
||||
if (future_goal_handle_) {
|
||||
RCLCPP_DEBUG(
|
||||
node_->get_logger(),
|
||||
"Goal result for %s available, but it hasn't received the goal response yet. "
|
||||
"It's probably a goal result for the last goal request", action_name_.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
// TODO(#1652): a work around until rcl_action interface is updated
|
||||
// if goal ids are not matched, the older goal call this callback so ignore the result
|
||||
// if matched, it must be processed (including aborted)
|
||||
if (this->goal_handle_->get_goal_id() == result.goal_id) {
|
||||
goal_result_available_ = true;
|
||||
result_ = result;
|
||||
}
|
||||
};
|
||||
send_goal_options.feedback_callback =
|
||||
[this](typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr,
|
||||
const std::shared_ptr<const typename ActionT::Feedback> feedback) {
|
||||
feedback_ = feedback;
|
||||
};
|
||||
|
||||
future_goal_handle_ = std::make_shared<
|
||||
std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>(
|
||||
action_client_->async_send_goal(goal_, send_goal_options));
|
||||
time_goal_sent_ = node_->now();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Function to check if the action server acknowledged a new goal
|
||||
* @param elapsed Duration since the last goal was sent and future goal handle has not completed.
|
||||
* After waiting for the future to complete, this value is incremented with the timeout value.
|
||||
* @return boolean True if future_goal_handle_ returns SUCCESS, False otherwise
|
||||
*/
|
||||
bool is_future_goal_handle_complete(std::chrono::milliseconds & elapsed)
|
||||
{
|
||||
auto remaining = server_timeout_ - elapsed;
|
||||
|
||||
// server has already timed out, no need to sleep
|
||||
if (remaining <= std::chrono::milliseconds(0)) {
|
||||
future_goal_handle_.reset();
|
||||
return false;
|
||||
}
|
||||
|
||||
auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining;
|
||||
auto result =
|
||||
callback_group_executor_.spin_until_future_complete(*future_goal_handle_, timeout);
|
||||
elapsed += timeout;
|
||||
|
||||
if (result == rclcpp::FutureReturnCode::INTERRUPTED) {
|
||||
future_goal_handle_.reset();
|
||||
throw std::runtime_error("send_goal failed");
|
||||
}
|
||||
|
||||
if (result == rclcpp::FutureReturnCode::SUCCESS) {
|
||||
goal_handle_ = future_goal_handle_->get();
|
||||
future_goal_handle_.reset();
|
||||
if (!goal_handle_) {
|
||||
throw std::runtime_error("Goal was rejected by the action server");
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Function to increment recovery count on blackboard if this node wraps a recovery
|
||||
*/
|
||||
void increment_recovery_count()
|
||||
{
|
||||
int recovery_count = 0;
|
||||
config().blackboard->template get<int>("number_recoveries", recovery_count); // NOLINT
|
||||
recovery_count += 1;
|
||||
config().blackboard->template set<int>("number_recoveries", recovery_count); // NOLINT
|
||||
}
|
||||
|
||||
std::string action_name_;
|
||||
typename std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_;
|
||||
|
||||
// All ROS2 actions have a goal and a result
|
||||
typename ActionT::Goal goal_;
|
||||
bool goal_updated_{false};
|
||||
bool goal_result_available_{false};
|
||||
typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr goal_handle_;
|
||||
typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult result_;
|
||||
|
||||
// To handle feedback from action server
|
||||
std::shared_ptr<const typename ActionT::Feedback> feedback_;
|
||||
|
||||
// The node that will be used for any ROS operations
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
rclcpp::CallbackGroup::SharedPtr callback_group_;
|
||||
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
|
||||
|
||||
// The timeout value while waiting for response from a server when a
|
||||
// new action goal is sent or canceled
|
||||
std::chrono::milliseconds server_timeout_;
|
||||
|
||||
// The timeout value for BT loop execution
|
||||
std::chrono::milliseconds max_timeout_;
|
||||
|
||||
// The timeout value for waiting for a service to response
|
||||
std::chrono::milliseconds wait_for_service_timeout_;
|
||||
|
||||
// To track the action server acknowledgement when a new goal is sent
|
||||
std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>
|
||||
future_goal_handle_;
|
||||
rclcpp::Time time_goal_sent_;
|
||||
|
||||
// Can be set in on_tick or on_wait_for_result to indicate if a goal should be sent.
|
||||
bool should_send_goal_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
|
||||
@@ -0,0 +1,251 @@
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav2_behavior_tree/behavior_tree_engine.hpp"
|
||||
#include "nav2_behavior_tree/ros_topic_logger.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_util/simple_action_server.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
/**
|
||||
* @class nav2_behavior_tree::BtActionServer
|
||||
* @brief An action server that uses behavior tree to execute an action
|
||||
*/
|
||||
template<class ActionT>
|
||||
class BtActionServer
|
||||
{
|
||||
public:
|
||||
using ActionServer = nav2_util::SimpleActionServer<ActionT>;
|
||||
|
||||
typedef std::function<bool (typename ActionT::Goal::ConstSharedPtr)> OnGoalReceivedCallback;
|
||||
typedef std::function<void ()> OnLoopCallback;
|
||||
typedef std::function<void (typename ActionT::Goal::ConstSharedPtr)> OnPreemptCallback;
|
||||
typedef std::function<void (typename ActionT::Result::SharedPtr,
|
||||
nav2_behavior_tree::BtStatus)> OnCompletionCallback;
|
||||
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::BtActionServer class
|
||||
*/
|
||||
explicit BtActionServer(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
const std::string & action_name,
|
||||
const std::vector<std::string> & plugin_lib_names,
|
||||
const std::string & default_bt_xml_filename,
|
||||
OnGoalReceivedCallback on_goal_received_callback,
|
||||
OnLoopCallback on_loop_callback,
|
||||
OnPreemptCallback on_preempt_callback,
|
||||
OnCompletionCallback on_completion_callback);
|
||||
|
||||
/**
|
||||
* @brief A destructor for nav2_behavior_tree::BtActionServer class
|
||||
*/
|
||||
~BtActionServer();
|
||||
|
||||
/**
|
||||
* @brief Configures member variables
|
||||
* Initializes action server for, builds behavior tree from xml file,
|
||||
* and calls user-defined onConfigure.
|
||||
* @return bool true on SUCCESS and false on FAILURE
|
||||
*/
|
||||
bool on_configure();
|
||||
|
||||
/**
|
||||
* @brief Activates action server
|
||||
* @return bool true on SUCCESS and false on FAILURE
|
||||
*/
|
||||
bool on_activate();
|
||||
|
||||
/**
|
||||
* @brief Deactivates action server
|
||||
* @return bool true on SUCCESS and false on FAILURE
|
||||
*/
|
||||
bool on_deactivate();
|
||||
|
||||
/**
|
||||
* @brief Resets member variables
|
||||
* @return bool true on SUCCESS and false on FAILURE
|
||||
*/
|
||||
bool on_cleanup();
|
||||
|
||||
/**
|
||||
* @brief Replace current BT with another one
|
||||
* @param bt_xml_filename The file containing the new BT, uses default filename if empty
|
||||
* @return bool true if the resulting BT correspond to the one in bt_xml_filename. false
|
||||
* if something went wrong, and previous BT is maintained
|
||||
*/
|
||||
bool loadBehaviorTree(const std::string & bt_xml_filename = "");
|
||||
|
||||
/**
|
||||
* @brief Getter function for BT Blackboard
|
||||
* @return BT::Blackboard::Ptr Shared pointer to current BT blackboard
|
||||
*/
|
||||
BT::Blackboard::Ptr getBlackboard() const
|
||||
{
|
||||
return blackboard_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Getter function for current BT XML filename
|
||||
* @return string Containing current BT XML filename
|
||||
*/
|
||||
std::string getCurrentBTFilename() const
|
||||
{
|
||||
return current_bt_xml_filename_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Getter function for default BT XML filename
|
||||
* @return string Containing default BT XML filename
|
||||
*/
|
||||
std::string getDefaultBTFilename() const
|
||||
{
|
||||
return default_bt_xml_filename_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Wrapper function to accept pending goal if a preempt has been requested
|
||||
* @return Shared pointer to pending action goal
|
||||
*/
|
||||
const std::shared_ptr<const typename ActionT::Goal> acceptPendingGoal()
|
||||
{
|
||||
return action_server_->accept_pending_goal();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Wrapper function to terminate pending goal if a preempt has been requested
|
||||
*/
|
||||
void terminatePendingGoal()
|
||||
{
|
||||
action_server_->terminate_pending_goal();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Wrapper function to get current goal
|
||||
* @return Shared pointer to current action goal
|
||||
*/
|
||||
const std::shared_ptr<const typename ActionT::Goal> getCurrentGoal() const
|
||||
{
|
||||
return action_server_->get_current_goal();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Wrapper function to get pending goal
|
||||
* @return Shared pointer to pending action goal
|
||||
*/
|
||||
const std::shared_ptr<const typename ActionT::Goal> getPendingGoal() const
|
||||
{
|
||||
return action_server_->get_pending_goal();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Wrapper function to publish action feedback
|
||||
*/
|
||||
void publishFeedback(typename std::shared_ptr<typename ActionT::Feedback> feedback)
|
||||
{
|
||||
action_server_->publish_feedback(feedback);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Getter function for the current BT tree
|
||||
* @return BT::Tree Current behavior tree
|
||||
*/
|
||||
const BT::Tree & getTree() const
|
||||
{
|
||||
return tree_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Function to halt the current tree. It will interrupt the execution of RUNNING nodes
|
||||
* by calling their halt() implementation (only for Async nodes that may return RUNNING)
|
||||
*/
|
||||
void haltTree()
|
||||
{
|
||||
tree_.rootNode()->halt();
|
||||
}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Action server callback
|
||||
*/
|
||||
void executeCallback();
|
||||
|
||||
// Action name
|
||||
std::string action_name_;
|
||||
|
||||
// Our action server implements the template action
|
||||
std::shared_ptr<ActionServer> action_server_;
|
||||
|
||||
// Behavior Tree to be executed when goal is received
|
||||
BT::Tree tree_;
|
||||
|
||||
// The blackboard shared by all of the nodes in the tree
|
||||
BT::Blackboard::Ptr blackboard_;
|
||||
|
||||
// The XML file that cointains the Behavior Tree to create
|
||||
std::string current_bt_xml_filename_;
|
||||
std::string default_bt_xml_filename_;
|
||||
|
||||
// The wrapper class for the BT functionality
|
||||
std::unique_ptr<nav2_behavior_tree::BehaviorTreeEngine> bt_;
|
||||
|
||||
// Libraries to pull plugins (BT Nodes) from
|
||||
std::vector<std::string> plugin_lib_names_;
|
||||
|
||||
// A regular, non-spinning ROS node that we can use for calls to the action client
|
||||
rclcpp::Node::SharedPtr client_node_;
|
||||
|
||||
// Parent node
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
|
||||
|
||||
// Clock
|
||||
rclcpp::Clock::SharedPtr clock_;
|
||||
|
||||
// Logger
|
||||
rclcpp::Logger logger_{rclcpp::get_logger("BtActionServer")};
|
||||
|
||||
// To publish BT logs
|
||||
std::unique_ptr<RosTopicLogger> topic_logger_;
|
||||
|
||||
// Duration for each iteration of BT execution
|
||||
std::chrono::milliseconds bt_loop_duration_;
|
||||
|
||||
// Default timeout value while waiting for response from a server
|
||||
std::chrono::milliseconds default_server_timeout_;
|
||||
|
||||
// The timeout value for waiting for a service to response
|
||||
std::chrono::milliseconds wait_for_service_timeout_;
|
||||
|
||||
// should the BT be reloaded even if the same xml filename is requested?
|
||||
bool always_reload_bt_xml_ = false;
|
||||
|
||||
// User-provided callbacks
|
||||
OnGoalReceivedCallback on_goal_received_callback_;
|
||||
OnLoopCallback on_loop_callback_;
|
||||
OnPreemptCallback on_preempt_callback_;
|
||||
OnCompletionCallback on_completion_callback_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include <nav2_behavior_tree/bt_action_server_impl.hpp> // NOLINT(build/include_order)
|
||||
#endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_
|
||||
@@ -0,0 +1,281 @@
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <fstream>
|
||||
#include <set>
|
||||
#include <exception>
|
||||
#include <vector>
|
||||
|
||||
#include "nav2_msgs/action/navigate_to_pose.hpp"
|
||||
#include "nav2_behavior_tree/bt_action_server.hpp"
|
||||
#include "ament_index_cpp/get_package_share_directory.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
template<class ActionT>
|
||||
BtActionServer<ActionT>::BtActionServer(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
const std::string & action_name,
|
||||
const std::vector<std::string> & plugin_lib_names,
|
||||
const std::string & default_bt_xml_filename,
|
||||
OnGoalReceivedCallback on_goal_received_callback,
|
||||
OnLoopCallback on_loop_callback,
|
||||
OnPreemptCallback on_preempt_callback,
|
||||
OnCompletionCallback on_completion_callback)
|
||||
: action_name_(action_name),
|
||||
default_bt_xml_filename_(default_bt_xml_filename),
|
||||
plugin_lib_names_(plugin_lib_names),
|
||||
node_(parent),
|
||||
on_goal_received_callback_(on_goal_received_callback),
|
||||
on_loop_callback_(on_loop_callback),
|
||||
on_preempt_callback_(on_preempt_callback),
|
||||
on_completion_callback_(on_completion_callback)
|
||||
{
|
||||
auto node = node_.lock();
|
||||
logger_ = node->get_logger();
|
||||
clock_ = node->get_clock();
|
||||
|
||||
// Declare this node's parameters
|
||||
if (!node->has_parameter("bt_loop_duration")) {
|
||||
node->declare_parameter("bt_loop_duration", 10);
|
||||
}
|
||||
if (!node->has_parameter("default_server_timeout")) {
|
||||
node->declare_parameter("default_server_timeout", 20);
|
||||
}
|
||||
if (!node->has_parameter("always_reload_bt_xml")) {
|
||||
node->declare_parameter("always_reload_bt_xml", false);
|
||||
}
|
||||
if (!node->has_parameter("wait_for_service_timeout")) {
|
||||
node->declare_parameter("wait_for_service_timeout", 1000);
|
||||
}
|
||||
}
|
||||
|
||||
template<class ActionT>
|
||||
BtActionServer<ActionT>::~BtActionServer()
|
||||
{}
|
||||
|
||||
template<class ActionT>
|
||||
bool BtActionServer<ActionT>::on_configure()
|
||||
{
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
// Name client node after action name
|
||||
std::string client_node_name = action_name_;
|
||||
std::replace(client_node_name.begin(), client_node_name.end(), '/', '_');
|
||||
// Use suffix '_rclcpp_node' to keep parameter file consistency #1773
|
||||
auto options = rclcpp::NodeOptions().arguments(
|
||||
{"--ros-args",
|
||||
"-r",
|
||||
std::string("__node:=") +
|
||||
std::string(node->get_name()) + "_" + client_node_name + "_rclcpp_node",
|
||||
"--"});
|
||||
|
||||
// Support for handling the topic-based goal pose from rviz
|
||||
client_node_ = std::make_shared<rclcpp::Node>("_", options);
|
||||
|
||||
// Declare parameters for common client node applications to share with BT nodes
|
||||
// Declare if not declared in case being used an external application, then copying
|
||||
// all of the main node's parameters to the client for BT nodes to obtain
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, "global_frame", rclcpp::ParameterValue(std::string("map")));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link")));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, "transform_tolerance", rclcpp::ParameterValue(0.1));
|
||||
nav2_util::copy_all_parameters(node, client_node_);
|
||||
|
||||
action_server_ = std::make_shared<ActionServer>(
|
||||
node->get_node_base_interface(),
|
||||
node->get_node_clock_interface(),
|
||||
node->get_node_logging_interface(),
|
||||
node->get_node_waitables_interface(),
|
||||
action_name_, std::bind(&BtActionServer<ActionT>::executeCallback, this));
|
||||
|
||||
// Get parameters for BT timeouts
|
||||
int timeout;
|
||||
node->get_parameter("bt_loop_duration", timeout);
|
||||
bt_loop_duration_ = std::chrono::milliseconds(timeout);
|
||||
node->get_parameter("default_server_timeout", timeout);
|
||||
default_server_timeout_ = std::chrono::milliseconds(timeout);
|
||||
int wait_for_service_timeout;
|
||||
node->get_parameter("wait_for_service_timeout", wait_for_service_timeout);
|
||||
wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout);
|
||||
node->get_parameter("always_reload_bt_xml", always_reload_bt_xml_);
|
||||
|
||||
// Create the class that registers our custom nodes and executes the BT
|
||||
bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_);
|
||||
|
||||
// Create the blackboard that will be shared by all of the nodes in the tree
|
||||
blackboard_ = BT::Blackboard::create();
|
||||
|
||||
// Put items on the blackboard
|
||||
blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_); // NOLINT
|
||||
blackboard_->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_); // NOLINT
|
||||
blackboard_->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_); // NOLINT
|
||||
blackboard_->set<std::chrono::milliseconds>(
|
||||
"wait_for_service_timeout",
|
||||
wait_for_service_timeout_);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
template<class ActionT>
|
||||
bool BtActionServer<ActionT>::on_activate()
|
||||
{
|
||||
if (!loadBehaviorTree(default_bt_xml_filename_)) {
|
||||
RCLCPP_ERROR(logger_, "Error loading XML file: %s", default_bt_xml_filename_.c_str());
|
||||
return false;
|
||||
}
|
||||
action_server_->activate();
|
||||
return true;
|
||||
}
|
||||
|
||||
template<class ActionT>
|
||||
bool BtActionServer<ActionT>::on_deactivate()
|
||||
{
|
||||
action_server_->deactivate();
|
||||
return true;
|
||||
}
|
||||
|
||||
template<class ActionT>
|
||||
bool BtActionServer<ActionT>::on_cleanup()
|
||||
{
|
||||
client_node_.reset();
|
||||
action_server_.reset();
|
||||
topic_logger_.reset();
|
||||
plugin_lib_names_.clear();
|
||||
current_bt_xml_filename_.clear();
|
||||
blackboard_.reset();
|
||||
bt_->haltAllActions(tree_.rootNode());
|
||||
bt_.reset();
|
||||
return true;
|
||||
}
|
||||
|
||||
template<class ActionT>
|
||||
bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filename)
|
||||
{
|
||||
// Empty filename is default for backward compatibility
|
||||
auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename;
|
||||
|
||||
// Use previous BT if it is the existing one and always reload flag is not set to true
|
||||
if (!always_reload_bt_xml_ && current_bt_xml_filename_ == filename) {
|
||||
RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml is already loaded");
|
||||
return true;
|
||||
}
|
||||
|
||||
// Read the input BT XML from the specified file into a string
|
||||
std::ifstream xml_file(filename);
|
||||
|
||||
if (!xml_file.good()) {
|
||||
RCLCPP_ERROR(logger_, "Couldn't open input XML file: %s", filename.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
auto xml_string = std::string(
|
||||
std::istreambuf_iterator<char>(xml_file),
|
||||
std::istreambuf_iterator<char>());
|
||||
|
||||
// Create the Behavior Tree from the XML input
|
||||
try {
|
||||
tree_ = bt_->createTreeFromText(xml_string, blackboard_);
|
||||
for (auto & blackboard : tree_.blackboard_stack) {
|
||||
blackboard->set<rclcpp::Node::SharedPtr>("node", client_node_);
|
||||
blackboard->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);
|
||||
blackboard->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
|
||||
blackboard->set<std::chrono::milliseconds>(
|
||||
"wait_for_service_timeout",
|
||||
wait_for_service_timeout_);
|
||||
}
|
||||
} catch (const std::exception & e) {
|
||||
RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what());
|
||||
return false;
|
||||
}
|
||||
|
||||
topic_logger_ = std::make_unique<RosTopicLogger>(client_node_, tree_);
|
||||
|
||||
current_bt_xml_filename_ = filename;
|
||||
return true;
|
||||
}
|
||||
|
||||
template<class ActionT>
|
||||
void BtActionServer<ActionT>::executeCallback()
|
||||
{
|
||||
if (!on_goal_received_callback_(action_server_->get_current_goal())) {
|
||||
action_server_->terminate_current();
|
||||
return;
|
||||
}
|
||||
|
||||
auto is_canceling = [&]() {
|
||||
if (action_server_ == nullptr) {
|
||||
RCLCPP_DEBUG(logger_, "Action server unavailable. Canceling.");
|
||||
return true;
|
||||
}
|
||||
if (!action_server_->is_server_active()) {
|
||||
RCLCPP_DEBUG(logger_, "Action server is inactive. Canceling.");
|
||||
return true;
|
||||
}
|
||||
return action_server_->is_cancel_requested();
|
||||
};
|
||||
|
||||
auto on_loop = [&]() {
|
||||
if (action_server_->is_preempt_requested() && on_preempt_callback_) {
|
||||
on_preempt_callback_(action_server_->get_pending_goal());
|
||||
}
|
||||
topic_logger_->flush();
|
||||
on_loop_callback_();
|
||||
};
|
||||
|
||||
// Execute the BT that was previously created in the configure step
|
||||
nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling, bt_loop_duration_);
|
||||
|
||||
// Make sure that the Bt is not in a running state from a previous execution
|
||||
// note: if all the ControlNodes are implemented correctly, this is not needed.
|
||||
bt_->haltAllActions(tree_.rootNode());
|
||||
|
||||
// Give server an opportunity to populate the result message or simple give
|
||||
// an indication that the action is complete.
|
||||
auto result = std::make_shared<typename ActionT::Result>();
|
||||
on_completion_callback_(result, rc);
|
||||
|
||||
switch (rc) {
|
||||
case nav2_behavior_tree::BtStatus::SUCCEEDED:
|
||||
action_server_->succeeded_current(result);
|
||||
RCLCPP_INFO(logger_, "Goal succeeded");
|
||||
break;
|
||||
|
||||
case nav2_behavior_tree::BtStatus::FAILED:
|
||||
action_server_->terminate_current(result);
|
||||
RCLCPP_ERROR(logger_, "Goal failed");
|
||||
break;
|
||||
|
||||
case nav2_behavior_tree::BtStatus::CANCELED:
|
||||
action_server_->terminate_all(result);
|
||||
RCLCPP_INFO(logger_, "Goal canceled");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_
|
||||
@@ -0,0 +1,179 @@
|
||||
// Copyright (c) 2022 Neobotix GmbH
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__BT_CANCEL_ACTION_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__BT_CANCEL_ACTION_NODE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <chrono>
|
||||
|
||||
#include "behaviortree_cpp_v3/action_node.h"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "nav2_behavior_tree/bt_conversions.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
using namespace std::chrono_literals; // NOLINT
|
||||
|
||||
/**
|
||||
* @brief Abstract class representing an action for cancelling BT node
|
||||
* @tparam ActionT Type of action
|
||||
*/
|
||||
template<class ActionT>
|
||||
class BtCancelActionNode : public BT::ActionNodeBase
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtCancelActionNode constructor
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
BtCancelActionNode(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name)
|
||||
{
|
||||
node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
|
||||
callback_group_ = node_->create_callback_group(
|
||||
rclcpp::CallbackGroupType::MutuallyExclusive,
|
||||
false);
|
||||
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
|
||||
|
||||
// Get the required items from the blackboard
|
||||
server_timeout_ =
|
||||
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
|
||||
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
|
||||
wait_for_service_timeout_ =
|
||||
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
|
||||
|
||||
std::string remapped_action_name;
|
||||
if (getInput("server_name", remapped_action_name)) {
|
||||
action_name_ = remapped_action_name;
|
||||
}
|
||||
createActionClient(action_name_);
|
||||
|
||||
// Give the derive class a chance to do any initialization
|
||||
RCLCPP_DEBUG(
|
||||
node_->get_logger(), "\"%s\" BtCancelActionNode initialized",
|
||||
xml_tag_name.c_str());
|
||||
}
|
||||
|
||||
BtCancelActionNode() = delete;
|
||||
|
||||
virtual ~BtCancelActionNode()
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Create instance of an action client
|
||||
* @param action_name Action name to create client for
|
||||
*/
|
||||
void createActionClient(const std::string & action_name)
|
||||
{
|
||||
// Now that we have the ROS node to use, create the action client for this BT action
|
||||
action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name, callback_group_);
|
||||
|
||||
// Make sure the server is actually there before continuing
|
||||
RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
|
||||
if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
|
||||
RCLCPP_ERROR(
|
||||
node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs",
|
||||
action_name.c_str(), wait_for_service_timeout_.count() / 1000.0);
|
||||
throw std::runtime_error(
|
||||
std::string("Action server ") + action_name +
|
||||
std::string(" not available"));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Any subclass of BtCancelActionNode that accepts parameters must provide a
|
||||
* providedPorts method and call providedBasicPorts in it.
|
||||
* @param addition Additional ports to add to BT port list
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedBasicPorts(BT::PortsList addition)
|
||||
{
|
||||
BT::PortsList basic = {
|
||||
BT::InputPort<std::string>("server_name", "Action server name"),
|
||||
BT::InputPort<std::chrono::milliseconds>("server_timeout")
|
||||
};
|
||||
basic.insert(addition.begin(), addition.end());
|
||||
|
||||
return basic;
|
||||
}
|
||||
|
||||
void halt() override
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts({});
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override
|
||||
{
|
||||
// setting the status to RUNNING to notify the BT Loggers (if any)
|
||||
setStatus(BT::NodeStatus::RUNNING);
|
||||
|
||||
// Cancel all the goals specified before 10ms from current time
|
||||
// to avoid async communication error
|
||||
|
||||
rclcpp::Time goal_expiry_time = node_->now() - std::chrono::milliseconds(10);
|
||||
|
||||
auto future_cancel = action_client_->async_cancel_goals_before(goal_expiry_time);
|
||||
|
||||
if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Failed to cancel the action server for %s", action_name_.c_str());
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
protected:
|
||||
std::string action_name_;
|
||||
typename std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_;
|
||||
|
||||
// The node that will be used for any ROS operations
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
rclcpp::CallbackGroup::SharedPtr callback_group_;
|
||||
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
|
||||
|
||||
// The timeout value while waiting for response from a server when a
|
||||
// new action goal is canceled
|
||||
std::chrono::milliseconds server_timeout_;
|
||||
// The timeout value for waiting for a service to response
|
||||
std::chrono::milliseconds wait_for_service_timeout_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__BT_CANCEL_ACTION_NODE_HPP_
|
||||
@@ -0,0 +1,116 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp/time.hpp"
|
||||
#include "behaviortree_cpp_v3/behavior_tree.h"
|
||||
#include "geometry_msgs/msg/point.hpp"
|
||||
#include "geometry_msgs/msg/quaternion.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
|
||||
namespace BT
|
||||
{
|
||||
|
||||
// The follow templates are required when using these types as parameters
|
||||
// in our BT XML files. They parse the strings in the XML into their corresponding
|
||||
// data type.
|
||||
|
||||
/**
|
||||
* @brief Parse XML string to geometry_msgs::msg::Point
|
||||
* @param key XML string
|
||||
* @return geometry_msgs::msg::Point
|
||||
*/
|
||||
template<>
|
||||
inline geometry_msgs::msg::Point convertFromString(const StringView key)
|
||||
{
|
||||
// three real numbers separated by semicolons
|
||||
auto parts = BT::splitString(key, ';');
|
||||
if (parts.size() != 3) {
|
||||
throw std::runtime_error("invalid number of fields for point attribute)");
|
||||
} else {
|
||||
geometry_msgs::msg::Point position;
|
||||
position.x = BT::convertFromString<double>(parts[0]);
|
||||
position.y = BT::convertFromString<double>(parts[1]);
|
||||
position.z = BT::convertFromString<double>(parts[2]);
|
||||
return position;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Parse XML string to geometry_msgs::msg::Quaternion
|
||||
* @param key XML string
|
||||
* @return geometry_msgs::msg::Quaternion
|
||||
*/
|
||||
template<>
|
||||
inline geometry_msgs::msg::Quaternion convertFromString(const StringView key)
|
||||
{
|
||||
// four real numbers separated by semicolons
|
||||
auto parts = BT::splitString(key, ';');
|
||||
if (parts.size() != 4) {
|
||||
throw std::runtime_error("invalid number of fields for orientation attribute)");
|
||||
} else {
|
||||
geometry_msgs::msg::Quaternion orientation;
|
||||
orientation.x = BT::convertFromString<double>(parts[0]);
|
||||
orientation.y = BT::convertFromString<double>(parts[1]);
|
||||
orientation.z = BT::convertFromString<double>(parts[2]);
|
||||
orientation.w = BT::convertFromString<double>(parts[3]);
|
||||
return orientation;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Parse XML string to geometry_msgs::msg::PoseStamped
|
||||
* @param key XML string
|
||||
* @return geometry_msgs::msg::PoseStamped
|
||||
*/
|
||||
template<>
|
||||
inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key)
|
||||
{
|
||||
// 7 real numbers separated by semicolons
|
||||
auto parts = BT::splitString(key, ';');
|
||||
if (parts.size() != 9) {
|
||||
throw std::runtime_error("invalid number of fields for PoseStamped attribute)");
|
||||
} else {
|
||||
geometry_msgs::msg::PoseStamped pose_stamped;
|
||||
pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[0]));
|
||||
pose_stamped.header.frame_id = BT::convertFromString<std::string>(parts[1]);
|
||||
pose_stamped.pose.position.x = BT::convertFromString<double>(parts[2]);
|
||||
pose_stamped.pose.position.y = BT::convertFromString<double>(parts[3]);
|
||||
pose_stamped.pose.position.z = BT::convertFromString<double>(parts[4]);
|
||||
pose_stamped.pose.orientation.x = BT::convertFromString<double>(parts[5]);
|
||||
pose_stamped.pose.orientation.y = BT::convertFromString<double>(parts[6]);
|
||||
pose_stamped.pose.orientation.z = BT::convertFromString<double>(parts[7]);
|
||||
pose_stamped.pose.orientation.w = BT::convertFromString<double>(parts[8]);
|
||||
return pose_stamped;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Parse XML string to std::chrono::milliseconds
|
||||
* @param key XML string
|
||||
* @return std::chrono::milliseconds
|
||||
*/
|
||||
template<>
|
||||
inline std::chrono::milliseconds convertFromString<std::chrono::milliseconds>(const StringView key)
|
||||
{
|
||||
return std::chrono::milliseconds(std::stoul(key.data()));
|
||||
}
|
||||
|
||||
} // namespace BT
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_
|
||||
@@ -0,0 +1,271 @@
|
||||
// Copyright (c) 2019 Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__BT_SERVICE_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__BT_SERVICE_NODE_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <chrono>
|
||||
|
||||
#include "behaviortree_cpp_v3/action_node.h"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_behavior_tree/bt_conversions.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
using namespace std::chrono_literals; // NOLINT
|
||||
|
||||
/**
|
||||
* @brief Abstract class representing a service based BT node
|
||||
* @tparam ServiceT Type of service
|
||||
*/
|
||||
template<class ServiceT>
|
||||
class BtServiceNode : public BT::ActionNodeBase
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtServiceNode constructor
|
||||
* @param service_node_name BT node name
|
||||
* @param conf BT node configuration
|
||||
* @param service_name Optional service name this node creates a client for instead of from input port
|
||||
*/
|
||||
BtServiceNode(
|
||||
const std::string & service_node_name,
|
||||
const BT::NodeConfiguration & conf,
|
||||
const std::string & service_name = "")
|
||||
: BT::ActionNodeBase(service_node_name, conf), service_name_(service_name), service_node_name_(
|
||||
service_node_name)
|
||||
{
|
||||
node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
|
||||
callback_group_ = node_->create_callback_group(
|
||||
rclcpp::CallbackGroupType::MutuallyExclusive,
|
||||
false);
|
||||
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
|
||||
|
||||
// Get the required items from the blackboard
|
||||
auto bt_loop_duration =
|
||||
config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
|
||||
server_timeout_ =
|
||||
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
|
||||
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
|
||||
wait_for_service_timeout_ =
|
||||
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
|
||||
|
||||
// timeout should be less than bt_loop_duration to be able to finish the current tick
|
||||
max_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(bt_loop_duration * 0.5);
|
||||
|
||||
// Now that we have node_ to use, create the service client for this BT service
|
||||
getInput("service_name", service_name_);
|
||||
service_client_ = node_->create_client<ServiceT>(
|
||||
service_name_,
|
||||
rclcpp::ServicesQoS().get_rmw_qos_profile(),
|
||||
callback_group_);
|
||||
|
||||
// Make a request for the service without parameter
|
||||
request_ = std::make_shared<typename ServiceT::Request>();
|
||||
|
||||
// Make sure the server is actually there before continuing
|
||||
RCLCPP_DEBUG(
|
||||
node_->get_logger(), "Waiting for \"%s\" service",
|
||||
service_name_.c_str());
|
||||
if (!service_client_->wait_for_service(wait_for_service_timeout_)) {
|
||||
RCLCPP_ERROR(
|
||||
node_->get_logger(), "\"%s\" service server not available after waiting for %.2fs",
|
||||
service_name_.c_str(), wait_for_service_timeout_.count() / 1000.0);
|
||||
throw std::runtime_error(
|
||||
std::string(
|
||||
"Service server %s not available",
|
||||
service_node_name.c_str()));
|
||||
}
|
||||
|
||||
RCLCPP_DEBUG(
|
||||
node_->get_logger(), "\"%s\" BtServiceNode initialized",
|
||||
service_node_name_.c_str());
|
||||
}
|
||||
|
||||
BtServiceNode() = delete;
|
||||
|
||||
virtual ~BtServiceNode()
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Any subclass of BtServiceNode that accepts parameters must provide a
|
||||
* providedPorts method and call providedBasicPorts in it.
|
||||
* @param addition Additional ports to add to BT port list
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedBasicPorts(BT::PortsList addition)
|
||||
{
|
||||
BT::PortsList basic = {
|
||||
BT::InputPort<std::string>("service_name", "please_set_service_name_in_BT_Node"),
|
||||
BT::InputPort<std::chrono::milliseconds>("server_timeout")
|
||||
};
|
||||
basic.insert(addition.begin(), addition.end());
|
||||
|
||||
return basic;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts({});
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT service
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override
|
||||
{
|
||||
if (!request_sent_) {
|
||||
// reset the flag to send the request or not,
|
||||
// allowing the user the option to set it in on_tick
|
||||
should_send_request_ = true;
|
||||
|
||||
// user defined callback, may modify "should_send_request_".
|
||||
on_tick();
|
||||
|
||||
if (!should_send_request_) {
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
future_result_ = service_client_->async_send_request(request_).share();
|
||||
sent_time_ = node_->now();
|
||||
request_sent_ = true;
|
||||
}
|
||||
return check_future();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief The other (optional) override required by a BT service.
|
||||
*/
|
||||
void halt() override
|
||||
{
|
||||
request_sent_ = false;
|
||||
setStatus(BT::NodeStatus::IDLE);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation on tick
|
||||
* Fill in service request with information if necessary
|
||||
*/
|
||||
virtual void on_tick()
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation upon successful
|
||||
* completion of the service. Could put a value on the blackboard.
|
||||
* @param response can be used to get the result of the service call in the BT Node.
|
||||
* @return BT::NodeStatus Returns SUCCESS by default, user may override to return another value
|
||||
*/
|
||||
virtual BT::NodeStatus on_completion(std::shared_ptr<typename ServiceT::Response>/*response*/)
|
||||
{
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Check the future and decide the status of BT
|
||||
* @return BT::NodeStatus SUCCESS if future complete before timeout, FAILURE otherwise
|
||||
*/
|
||||
virtual BT::NodeStatus check_future()
|
||||
{
|
||||
auto elapsed = (node_->now() - sent_time_).template to_chrono<std::chrono::milliseconds>();
|
||||
auto remaining = server_timeout_ - elapsed;
|
||||
|
||||
if (remaining > std::chrono::milliseconds(0)) {
|
||||
auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining;
|
||||
|
||||
rclcpp::FutureReturnCode rc;
|
||||
rc = callback_group_executor_.spin_until_future_complete(future_result_, timeout);
|
||||
if (rc == rclcpp::FutureReturnCode::SUCCESS) {
|
||||
request_sent_ = false;
|
||||
BT::NodeStatus status = on_completion(future_result_.get());
|
||||
return status;
|
||||
}
|
||||
|
||||
if (rc == rclcpp::FutureReturnCode::TIMEOUT) {
|
||||
on_wait_for_result();
|
||||
elapsed = (node_->now() - sent_time_).template to_chrono<std::chrono::milliseconds>();
|
||||
if (elapsed < server_timeout_) {
|
||||
return BT::NodeStatus::RUNNING;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_WARN(
|
||||
node_->get_logger(),
|
||||
"Node timed out while executing service call to %s.", service_name_.c_str());
|
||||
request_sent_ = false;
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation after a timeout waiting
|
||||
* for a result that hasn't been received yet
|
||||
*/
|
||||
virtual void on_wait_for_result()
|
||||
{
|
||||
}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Function to increment recovery count on blackboard if this node wraps a recovery
|
||||
*/
|
||||
void increment_recovery_count()
|
||||
{
|
||||
int recovery_count = 0;
|
||||
config().blackboard->template get<int>("number_recoveries", recovery_count); // NOLINT
|
||||
recovery_count += 1;
|
||||
config().blackboard->template set<int>("number_recoveries", recovery_count); // NOLINT
|
||||
}
|
||||
|
||||
std::string service_name_, service_node_name_;
|
||||
typename std::shared_ptr<rclcpp::Client<ServiceT>> service_client_;
|
||||
std::shared_ptr<typename ServiceT::Request> request_;
|
||||
|
||||
// The node that will be used for any ROS operations
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
rclcpp::CallbackGroup::SharedPtr callback_group_;
|
||||
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
|
||||
|
||||
// The timeout value while to use in the tick loop while waiting for
|
||||
// a result from the server
|
||||
std::chrono::milliseconds server_timeout_;
|
||||
|
||||
// The timeout value for BT loop execution
|
||||
std::chrono::milliseconds max_timeout_;
|
||||
|
||||
// The timeout value for waiting for a service to response
|
||||
std::chrono::milliseconds wait_for_service_timeout_;
|
||||
|
||||
// To track the server response when a new request is sent
|
||||
std::shared_future<typename ServiceT::Response::SharedPtr> future_result_;
|
||||
bool request_sent_{false};
|
||||
rclcpp::Time sent_time_;
|
||||
|
||||
// Can be set in on_tick or on_wait_for_result to indicate if a request should be sent.
|
||||
bool should_send_request_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__BT_SERVICE_NODE_HPP_
|
||||
+70
@@ -0,0 +1,70 @@
|
||||
// Copyright (c) 2022 Joshua Wallace
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_ACTION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_ACTION_HPP_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "nav2_behavior_tree/bt_action_node.hpp"
|
||||
#include "nav2_msgs/action/assisted_teleop.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::AssistedTeleop
|
||||
*/
|
||||
class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTeleop>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::nav2_msgs::action::AssistedTeleop
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
AssistedTeleopAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation on tick
|
||||
*/
|
||||
void on_tick() override;
|
||||
|
||||
BT::NodeStatus on_aborted() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
BT::InputPort<double>("time_allowance", 10.0, "Allowed time for running assisted teleop"),
|
||||
BT::InputPort<bool>("is_recovery", false, "If true the recovery count will be incremented")
|
||||
});
|
||||
}
|
||||
|
||||
private:
|
||||
bool is_recovery_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_ACTION_HPP_
|
||||
+59
@@ -0,0 +1,59 @@
|
||||
// Copyright (c) 2022 Joshua Wallace
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_CANCEL_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_CANCEL_NODE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "nav2_msgs/action/assisted_teleop.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp
|
||||
*/
|
||||
class AssistedTeleopCancel : public BtCancelActionNode<nav2_msgs::action::AssistedTeleop>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::BackUpAction
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
AssistedTeleopCancel(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_CANCEL_NODE_HPP_
|
||||
+65
@@ -0,0 +1,65 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_ACTION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_ACTION_HPP_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "nav2_behavior_tree/bt_action_node.hpp"
|
||||
#include "nav2_msgs/action/back_up.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp
|
||||
*/
|
||||
class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::BackUpAction
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
BackUpAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation on tick
|
||||
*/
|
||||
void on_tick() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
BT::InputPort<double>("backup_dist", 0.15, "Distance to backup"),
|
||||
BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup"),
|
||||
BT::InputPort<double>("time_allowance", 10.0, "Allowed time for reversing")
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_ACTION_HPP_
|
||||
+59
@@ -0,0 +1,59 @@
|
||||
// Copyright (c) 2022 Neobotix GmbH
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_CANCEL_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_CANCEL_NODE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "nav2_msgs/action/back_up.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp
|
||||
*/
|
||||
class BackUpCancel : public BtCancelActionNode<nav2_msgs::action::BackUp>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::BackUpAction
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
BackUpCancel(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_CANCEL_NODE_HPP_
|
||||
+127
@@ -0,0 +1,127 @@
|
||||
// Copyright (c) 2019 Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "nav2_behavior_tree/bt_service_node.hpp"
|
||||
#include "nav2_msgs/srv/clear_entire_costmap.hpp"
|
||||
#include "nav2_msgs/srv/clear_costmap_around_robot.hpp"
|
||||
#include "nav2_msgs/srv/clear_costmap_except_region.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtServiceNode class that wraps nav2_msgs::srv::ClearEntireCostmap
|
||||
*/
|
||||
class ClearEntireCostmapService : public BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::ClearEntireCostmapService
|
||||
* @param service_node_name Service name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
ClearEntireCostmapService(
|
||||
const std::string & service_node_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT service
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
void on_tick() override;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtServiceNode class that
|
||||
* wraps nav2_msgs::srv::ClearCostmapExceptRegion
|
||||
*/
|
||||
class ClearCostmapExceptRegionService
|
||||
: public BtServiceNode<nav2_msgs::srv::ClearCostmapExceptRegion>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::ClearCostmapExceptRegionService
|
||||
* @param service_node_name Service name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
ClearCostmapExceptRegionService(
|
||||
const std::string & service_node_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT service
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
void on_tick() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
BT::InputPort<double>(
|
||||
"reset_distance", 1,
|
||||
"Distance from the robot above which obstacles are cleared")
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtServiceNode class that
|
||||
* wraps nav2_msgs::srv::ClearCostmapAroundRobot
|
||||
*/
|
||||
class ClearCostmapAroundRobotService : public BtServiceNode<nav2_msgs::srv::ClearCostmapAroundRobot>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::ClearCostmapAroundRobotService
|
||||
* @param service_node_name Service name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
ClearCostmapAroundRobotService(
|
||||
const std::string & service_node_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT service
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
void on_tick() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
BT::InputPort<double>(
|
||||
"reset_distance", 1,
|
||||
"Distance from the robot under which obstacles are cleared")
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_
|
||||
+90
@@ -0,0 +1,90 @@
|
||||
// Copyright (c) 2021 Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_THROUGH_POSES_ACTION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_THROUGH_POSES_ACTION_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "nav2_msgs/action/compute_path_through_poses.hpp"
|
||||
#include "nav_msgs/msg/path.h"
|
||||
#include "nav2_behavior_tree/bt_action_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputePathThroughPoses
|
||||
*/
|
||||
class ComputePathThroughPosesAction
|
||||
: public BtActionNode<nav2_msgs::action::ComputePathThroughPoses>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::ComputePathThroughPosesAction
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
ComputePathThroughPosesAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation on tick
|
||||
*/
|
||||
void on_tick() override;
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation upon successful completion of the action
|
||||
*/
|
||||
BT::NodeStatus on_success() override;
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation upon abortion of the action
|
||||
*/
|
||||
BT::NodeStatus on_aborted() override;
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation upon cancelation of the action
|
||||
*/
|
||||
BT::NodeStatus on_cancelled() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathThroughPoses node"),
|
||||
BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
|
||||
"goals",
|
||||
"Destinations to plan through"),
|
||||
BT::InputPort<geometry_msgs::msg::PoseStamped>(
|
||||
"start", "Start pose of the path if overriding current robot pose"),
|
||||
BT::InputPort<std::string>(
|
||||
"planner_id", "",
|
||||
"Mapped name to the planner plugin type to use"),
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_THROUGH_POSES_ACTION_HPP_
|
||||
+90
@@ -0,0 +1,90 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "nav2_msgs/action/compute_path_to_pose.hpp"
|
||||
#include "nav_msgs/msg/path.h"
|
||||
#include "nav2_behavior_tree/bt_action_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputePathToPose
|
||||
*/
|
||||
class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePathToPose>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::ComputePathToPoseAction
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
ComputePathToPoseAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation on tick
|
||||
*/
|
||||
void on_tick() override;
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation upon successful completion of the action
|
||||
*/
|
||||
BT::NodeStatus on_success() override;
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation upon abortion of the action
|
||||
*/
|
||||
BT::NodeStatus on_aborted() override;
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation upon cancelation of the action
|
||||
*/
|
||||
BT::NodeStatus on_cancelled() override;
|
||||
|
||||
/**
|
||||
* \brief Override required by the a BT action. Cancel the action and set the path output
|
||||
*/
|
||||
void halt() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
|
||||
BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
|
||||
BT::InputPort<geometry_msgs::msg::PoseStamped>(
|
||||
"start", "Start pose of the path if overriding current robot pose"),
|
||||
BT::InputPort<std::string>(
|
||||
"planner_id", "",
|
||||
"Mapped name to the planner plugin type to use"),
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_
|
||||
+59
@@ -0,0 +1,59 @@
|
||||
// Copyright (c) 2022 Neobotix GmbH
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_CANCEL_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_CANCEL_NODE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "nav2_msgs/action/follow_path.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::FollowPath
|
||||
*/
|
||||
class ControllerCancel : public BtCancelActionNode<nav2_msgs::action::FollowPath>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::FollowPathAction
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
ControllerCancel(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_CANCEL_NODE_HPP_
|
||||
+99
@@ -0,0 +1,99 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
// Copyright (c) 2020 Pablo Iñigo Blasco
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_SELECTOR_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_SELECTOR_NODE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
#include "behaviortree_cpp_v3/action_node.h"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief The ControllerSelector behavior is used to switch the controller
|
||||
* that will be used by the controller server. It subscribes to a topic "controller_selector"
|
||||
* to get the decision about what controller must be used. It is usually used before of
|
||||
* the FollowPath. The selected_controller output port is passed to controller_id
|
||||
* input port of the FollowPath
|
||||
*/
|
||||
class ControllerSelector : public BT::SyncActionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::ControllerSelector
|
||||
*
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
ControllerSelector(
|
||||
const std::string & xml_tag_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<std::string>(
|
||||
"default_controller",
|
||||
"the default controller to use if there is not any external topic message received."),
|
||||
|
||||
BT::InputPort<std::string>(
|
||||
"topic_name",
|
||||
"controller_selector",
|
||||
"the input topic name to select the controller"),
|
||||
|
||||
BT::OutputPort<std::string>(
|
||||
"selected_controller",
|
||||
"Selected controller by subscription")
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation on tick
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief callback function for the controller_selector topic
|
||||
*
|
||||
* @param msg the message with the id of the controller_selector
|
||||
*/
|
||||
void callbackControllerSelect(const std_msgs::msg::String::SharedPtr msg);
|
||||
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr controller_selector_sub_;
|
||||
|
||||
std::string last_selected_controller_;
|
||||
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
rclcpp::CallbackGroup::SharedPtr callback_group_;
|
||||
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
|
||||
|
||||
std::string topic_name_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_SELECTOR_NODE_HPP_
|
||||
+60
@@ -0,0 +1,60 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_ACTION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_ACTION_HPP_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "nav2_behavior_tree/bt_action_node.hpp"
|
||||
#include "nav2_msgs/action/drive_on_heading.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::DriveOnHeading
|
||||
*/
|
||||
class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeading>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::DriveOnHeadingAction
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
DriveOnHeadingAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
BT::InputPort<double>("dist_to_travel", 0.15, "Distance to travel"),
|
||||
BT::InputPort<double>("speed", 0.025, "Speed at which to travel"),
|
||||
BT::InputPort<double>("time_allowance", 10.0, "Allowed time for driving on heading")
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_ACTION_HPP_
|
||||
+59
@@ -0,0 +1,59 @@
|
||||
// Copyright (c) 2022 Neobotix GmbH
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_CANCEL_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_CANCEL_NODE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "nav2_msgs/action/drive_on_heading.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::DriveOnHeading
|
||||
*/
|
||||
class DriveOnHeadingCancel : public BtCancelActionNode<nav2_msgs::action::DriveOnHeading>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::DriveOnHeadingCancel
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
DriveOnHeadingCancel(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_CANCEL_NODE_HPP_
|
||||
+74
@@ -0,0 +1,74 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "nav2_msgs/action/follow_path.hpp"
|
||||
#include "nav2_behavior_tree/bt_action_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::FollowPath
|
||||
*/
|
||||
class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::FollowPathAction
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
FollowPathAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation on tick
|
||||
*/
|
||||
void on_tick() override;
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation after a timeout
|
||||
* waiting for a result that hasn't been received yet
|
||||
* @param feedback shared_ptr to latest feedback message
|
||||
*/
|
||||
void on_wait_for_result(
|
||||
std::shared_ptr<const nav2_msgs::action::FollowPath::Feedback> feedback) override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
|
||||
BT::InputPort<std::string>("controller_id", ""),
|
||||
BT::InputPort<std::string>("goal_checker_id", ""),
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_
|
||||
+55
@@ -0,0 +1,55 @@
|
||||
// Copyright (c) 2024 Marc Morcos
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_
|
||||
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
#include "behaviortree_cpp_v3/action_node.h"
|
||||
#include "nav_msgs/msg/path.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
class GetPoseFromPath : public BT::ActionNodeBase
|
||||
{
|
||||
public:
|
||||
GetPoseFromPath(
|
||||
const std::string & xml_tag_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<nav_msgs::msg::Path>("path", "Path to extract pose from"),
|
||||
BT::OutputPort<geometry_msgs::msg::PoseStamped>("pose", "Stamped Extracted Pose"),
|
||||
BT::InputPort<int>("index", 0, "Index of pose to extract from. -1 is end of list"),
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
void halt() override {}
|
||||
BT::NodeStatus tick() override;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_
|
||||
+97
@@ -0,0 +1,97 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
// Copyright (c) 2020 Pablo Iñigo Blasco
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GOAL_CHECKER_SELECTOR_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GOAL_CHECKER_SELECTOR_NODE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
#include "behaviortree_cpp_v3/action_node.h"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief The GoalCheckerSelector behavior is used to switch the goal checker
|
||||
* of the controller server. It subscribes to a topic "goal_checker_selector"
|
||||
* to get the decision about what goal_checker must be used. It is usually used before of
|
||||
* the FollowPath. The selected_goal_checker output port is passed to goal_checker_id
|
||||
* input port of the FollowPath
|
||||
*/
|
||||
class GoalCheckerSelector : public BT::SyncActionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::GoalCheckerSelector
|
||||
*
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
GoalCheckerSelector(
|
||||
const std::string & xml_tag_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<std::string>(
|
||||
"default_goal_checker",
|
||||
"the default goal_checker to use if there is not any external topic message received."),
|
||||
|
||||
BT::InputPort<std::string>(
|
||||
"topic_name",
|
||||
"goal_checker_selector",
|
||||
"the input topic name to select the goal_checker"),
|
||||
|
||||
BT::OutputPort<std::string>(
|
||||
"selected_goal_checker",
|
||||
"Selected goal_checker by subscription")
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation on tick
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief callback function for the goal_checker_selector topic
|
||||
*
|
||||
* @param msg the message with the id of the goal_checker_selector
|
||||
*/
|
||||
void callbackGoalCheckerSelect(const std_msgs::msg::String::SharedPtr msg);
|
||||
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr goal_checker_selector_sub_;
|
||||
|
||||
std::string last_selected_goal_checker_;
|
||||
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
|
||||
std::string topic_name_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GOAL_CHECKER_SELECTOR_NODE_HPP_
|
||||
+66
@@ -0,0 +1,66 @@
|
||||
// Copyright (c) 2021 Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_THROUGH_POSES_ACTION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_THROUGH_POSES_ACTION_HPP_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "geometry_msgs/msg/point.hpp"
|
||||
#include "geometry_msgs/msg/quaternion.hpp"
|
||||
#include "nav2_msgs/action/navigate_through_poses.hpp"
|
||||
#include "nav2_behavior_tree/bt_action_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::NavigateThroughPoses
|
||||
*/
|
||||
class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::NavigateThroughPoses>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::NavigateThroughPosesAction
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
NavigateThroughPosesAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation on tick
|
||||
*/
|
||||
void on_tick() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
BT::InputPort<geometry_msgs::msg::PoseStamped>("goals", "Destinations to plan through"),
|
||||
BT::InputPort<std::string>("behavior_tree", "Behavior tree to run"),
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_THROUGH_POSES_ACTION_HPP_
|
||||
+66
@@ -0,0 +1,66 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_TO_POSE_ACTION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_TO_POSE_ACTION_HPP_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "geometry_msgs/msg/point.hpp"
|
||||
#include "geometry_msgs/msg/quaternion.hpp"
|
||||
#include "nav2_msgs/action/navigate_to_pose.hpp"
|
||||
#include "nav2_behavior_tree/bt_action_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::NavigateToPose
|
||||
*/
|
||||
class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPose>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::NavigateToPoseAction
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
NavigateToPoseAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation on tick
|
||||
*/
|
||||
void on_tick() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
|
||||
BT::InputPort<std::string>("behavior_tree", "Behavior tree to run"),
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_TO_POSE_ACTION_HPP_
|
||||
+100
@@ -0,0 +1,100 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
// Copyright (c) 2020 Pablo Iñigo Blasco
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PLANNER_SELECTOR_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PLANNER_SELECTOR_NODE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
#include "behaviortree_cpp_v3/action_node.h"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief The PlannerSelector behavior is used to switch the planner
|
||||
* that will be used by the planner server. It subscribes to a topic "planner_selector"
|
||||
* to get the decision about what planner must be used. It is usually used before of
|
||||
* the ComputePathToPoseAction. The selected_planner output port is passed to planner_id
|
||||
* input port of the ComputePathToPoseAction
|
||||
*/
|
||||
class PlannerSelector : public BT::SyncActionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::PlannerSelector
|
||||
*
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
PlannerSelector(
|
||||
const std::string & xml_tag_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<std::string>(
|
||||
"default_planner",
|
||||
"the default planner to use if there is not any external topic message received."),
|
||||
|
||||
BT::InputPort<std::string>(
|
||||
"topic_name",
|
||||
"planner_selector",
|
||||
"the input topic name to select the planner"),
|
||||
|
||||
BT::OutputPort<std::string>(
|
||||
"selected_planner",
|
||||
"Selected planner by subscription")
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation on tick
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief callback function for the planner_selector topic
|
||||
*
|
||||
* @param msg the message with the id of the planner_selector
|
||||
*/
|
||||
void callbackPlannerSelect(const std_msgs::msg::String::SharedPtr msg);
|
||||
|
||||
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr planner_selector_sub_;
|
||||
|
||||
std::string last_selected_planner_;
|
||||
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
rclcpp::CallbackGroup::SharedPtr callback_group_;
|
||||
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
|
||||
|
||||
std::string topic_name_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PLANNER_SELECTOR_NODE_HPP_
|
||||
+96
@@ -0,0 +1,96 @@
|
||||
// Copyright (c) 2024 Open Navigation LLC
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_SELECTOR_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_SELECTOR_NODE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
#include "behaviortree_cpp_v3/action_node.h"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief The ProgressCheckerSelector behavior is used to switch the progress checker
|
||||
* of the controller server. It subscribes to a topic "progress_checker_selector"
|
||||
* to get the decision about what progress_checker must be used. It is usually used before of
|
||||
* the FollowPath. The selected_progress_checker output port is passed to progress_checker_id
|
||||
* input port of the FollowPath
|
||||
*/
|
||||
class ProgressCheckerSelector : public BT::SyncActionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::ProgressCheckerSelector
|
||||
*
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
ProgressCheckerSelector(
|
||||
const std::string & xml_tag_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<std::string>(
|
||||
"default_progress_checker",
|
||||
"the default progress_checker to use if there is not any external topic message received."),
|
||||
|
||||
BT::InputPort<std::string>(
|
||||
"topic_name",
|
||||
"progress_checker_selector",
|
||||
"the input topic name to select the progress_checker"),
|
||||
|
||||
BT::OutputPort<std::string>(
|
||||
"selected_progress_checker",
|
||||
"Selected progress_checker by subscription")
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation on tick
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief callback function for the progress_checker_selector topic
|
||||
*
|
||||
* @param msg the message with the id of the progress_checker_selector
|
||||
*/
|
||||
void callbackProgressCheckerSelect(const std_msgs::msg::String::SharedPtr msg);
|
||||
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr progress_checker_selector_sub_;
|
||||
|
||||
std::string last_selected_progress_checker_;
|
||||
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
|
||||
std::string topic_name_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_SELECTOR_NODE_HPP_
|
||||
+44
@@ -0,0 +1,44 @@
|
||||
// Copyright (c) 2019 Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "nav2_behavior_tree/bt_service_node.hpp"
|
||||
#include "std_srvs/srv/empty.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtServiceNode class that wraps nav2_msgs::srv::Empty
|
||||
*/
|
||||
class ReinitializeGlobalLocalizationService : public BtServiceNode<std_srvs::srv::Empty>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::ReinitializeGlobalLocalizationService
|
||||
* @param service_node_name Service name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
ReinitializeGlobalLocalizationService(
|
||||
const std::string & service_node_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
|
||||
+63
@@ -0,0 +1,63 @@
|
||||
// Copyright (c) 2021 Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_PASSED_GOALS_ACTION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_PASSED_GOALS_ACTION_HPP_
|
||||
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
#include "behaviortree_cpp_v3/action_node.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
class RemovePassedGoals : public BT::ActionNodeBase
|
||||
{
|
||||
public:
|
||||
typedef std::vector<geometry_msgs::msg::PoseStamped> Goals;
|
||||
|
||||
RemovePassedGoals(
|
||||
const std::string & xml_tag_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<Goals>("input_goals", "Original goals to remove viapoints from"),
|
||||
BT::OutputPort<Goals>("output_goals", "Goals with passed viapoints removed"),
|
||||
BT::InputPort<double>("radius", 0.5, "radius to goal for it to be considered for removal"),
|
||||
BT::InputPort<std::string>("global_frame", std::string("map"), "Global frame"),
|
||||
BT::InputPort<std::string>("robot_base_frame", std::string("base_link"), "Robot base frame"),
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
void halt() override {}
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
double viapoint_achieved_radius_;
|
||||
std::string robot_base_frame_, global_frame_;
|
||||
double transform_tolerance_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_PASSED_GOALS_ACTION_HPP_
|
||||
+81
@@ -0,0 +1,81 @@
|
||||
// Copyright (c) 2021 RoboTech Vision
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTH_PATH_ACTION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTH_PATH_ACTION_HPP_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "nav2_msgs/action/smooth_path.hpp"
|
||||
#include "nav_msgs/msg/path.h"
|
||||
#include "nav2_behavior_tree/bt_action_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::SmoothPath
|
||||
*/
|
||||
class SmoothPathAction : public nav2_behavior_tree::BtActionNode<nav2_msgs::action::SmoothPath>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::SmoothPathAction
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
SmoothPathAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation on tick
|
||||
*/
|
||||
void on_tick() override;
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation upon successful completion of the action
|
||||
*/
|
||||
BT::NodeStatus on_success() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
BT::OutputPort<nav_msgs::msg::Path>(
|
||||
"smoothed_path",
|
||||
"Path smoothed by SmootherServer node"),
|
||||
BT::OutputPort<double>("smoothing_duration", "Time taken to smooth path"),
|
||||
BT::OutputPort<bool>(
|
||||
"was_completed", "True if smoothing was not interrupted by time limit"),
|
||||
BT::InputPort<nav_msgs::msg::Path>("unsmoothed_path", "Path to be smoothed"),
|
||||
BT::InputPort<double>("max_smoothing_duration", 3.0, "Maximum smoothing duration"),
|
||||
BT::InputPort<bool>(
|
||||
"check_for_collisions", false,
|
||||
"If true collision check will be performed after smoothing"),
|
||||
BT::InputPort<std::string>("smoother_id", ""),
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTH_PATH_ACTION_HPP_
|
||||
+99
@@ -0,0 +1,99 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
// Copyright (c) 2020 Pablo Iñigo Blasco
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTHER_SELECTOR_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTHER_SELECTOR_NODE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
#include "behaviortree_cpp_v3/action_node.h"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief The SmootherSelector behavior is used to switch the smoother
|
||||
* that will be used by the smoother server. It subscribes to a topic "smoother_selector"
|
||||
* to get the decision about what smoother must be used. It is usually used before of
|
||||
* the FollowPath. The selected_smoother output port is passed to smoother_id
|
||||
* input port of the FollowPath
|
||||
*/
|
||||
class SmootherSelector : public BT::SyncActionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::SmootherSelector
|
||||
*
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
SmootherSelector(
|
||||
const std::string & xml_tag_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<std::string>(
|
||||
"default_smoother",
|
||||
"the default smoother to use if there is not any external topic message received."),
|
||||
|
||||
BT::InputPort<std::string>(
|
||||
"topic_name",
|
||||
"smoother_selector",
|
||||
"the input topic name to select the smoother"),
|
||||
|
||||
BT::OutputPort<std::string>(
|
||||
"selected_smoother",
|
||||
"Selected smoother by subscription")
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation on tick
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief callback function for the smoother_selector topic
|
||||
*
|
||||
* @param msg the message with the id of the smoother_selector
|
||||
*/
|
||||
void callbackSmootherSelect(const std_msgs::msg::String::SharedPtr msg);
|
||||
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr smoother_selector_sub_;
|
||||
|
||||
std::string last_selected_smoother_;
|
||||
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
rclcpp::CallbackGroup::SharedPtr callback_group_;
|
||||
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
|
||||
|
||||
std::string topic_name_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTHER_SELECTOR_NODE_HPP_
|
||||
+68
@@ -0,0 +1,68 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_ACTION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_ACTION_HPP_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "nav2_behavior_tree/bt_action_node.hpp"
|
||||
#include "nav2_msgs/action/spin.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Spin
|
||||
*/
|
||||
class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::SpinAction
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
SpinAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation on tick
|
||||
*/
|
||||
void on_tick() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
BT::InputPort<double>("spin_dist", 1.57, "Spin distance"),
|
||||
BT::InputPort<double>("time_allowance", 10.0, "Allowed time for spinning"),
|
||||
BT::InputPort<bool>("is_recovery", true, "True if recovery")
|
||||
});
|
||||
}
|
||||
|
||||
private:
|
||||
bool is_recovery_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_ACTION_HPP_
|
||||
+59
@@ -0,0 +1,59 @@
|
||||
// Copyright (c) 2022 Neobotix GmbH
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_CANCEL_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_CANCEL_NODE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "nav2_msgs/action/spin.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait
|
||||
*/
|
||||
class SpinCancel : public BtCancelActionNode<nav2_msgs::action::Spin>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::WaitAction
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
SpinCancel(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_CANCEL_NODE_HPP_
|
||||
+74
@@ -0,0 +1,74 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
// Copyright (c) 2020 Francisco Martin Rico
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
|
||||
#include "behaviortree_cpp_v3/action_node.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::ActionNodeBase to shorten path by some distance
|
||||
*/
|
||||
class TruncatePath : public BT::ActionNodeBase
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::TruncatePath constructor
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
TruncatePath(
|
||||
const std::string & xml_tag_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<nav_msgs::msg::Path>("input_path", "Original Path"),
|
||||
BT::OutputPort<nav_msgs::msg::Path>("output_path", "Path truncated to a certain distance"),
|
||||
BT::InputPort<double>("distance", 1.0, "distance"),
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief The other (optional) override required by a BT action.
|
||||
*/
|
||||
void halt() override {}
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
double distance_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_
|
||||
+124
@@ -0,0 +1,124 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
// Copyright (c) 2020 Francisco Martin Rico
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_LOCAL_ACTION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_LOCAL_ACTION_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <limits>
|
||||
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
|
||||
#include "behaviortree_cpp_v3/action_node.h"
|
||||
#include "tf2_ros/buffer.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::ActionNodeBase to shorten path to some distance around robot
|
||||
*/
|
||||
class TruncatePathLocal : public BT::ActionNodeBase
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::TruncatePathLocal constructor
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
TruncatePathLocal(
|
||||
const std::string & xml_tag_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<nav_msgs::msg::Path>("input_path", "Original Path"),
|
||||
BT::OutputPort<nav_msgs::msg::Path>(
|
||||
"output_path", "Path truncated to a certain distance around robot"),
|
||||
BT::InputPort<double>(
|
||||
"distance_forward", 8.0,
|
||||
"Distance in forward direction"),
|
||||
BT::InputPort<double>(
|
||||
"distance_backward", 4.0,
|
||||
"Distance in backward direction"),
|
||||
BT::InputPort<std::string>(
|
||||
"robot_frame", "base_link",
|
||||
"Robot base frame id"),
|
||||
BT::InputPort<double>(
|
||||
"transform_tolerance", 0.2,
|
||||
"Transform lookup tolerance"),
|
||||
BT::InputPort<geometry_msgs::msg::PoseStamped>(
|
||||
"pose", "Manually specified pose to be used"
|
||||
"if overriding current robot pose"),
|
||||
BT::InputPort<double>(
|
||||
"angular_distance_weight", 0.0,
|
||||
"Weight of angular distance relative to positional distance when finding which path "
|
||||
"pose is closest to robot. Not applicable on paths without orientations assigned"),
|
||||
BT::InputPort<double>(
|
||||
"max_robot_pose_search_dist", std::numeric_limits<double>::infinity(),
|
||||
"Maximum forward integrated distance along the path (starting from the last detected pose) "
|
||||
"to bound the search for the closest pose to the robot. When set to infinity (default), "
|
||||
"whole path is searched every time"),
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief The other (optional) override required by a BT action.
|
||||
*/
|
||||
void halt() override {}
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief Get either specified input pose or robot pose in path frame
|
||||
* @param path_frame_id Frame ID of path
|
||||
* @param pose Output pose
|
||||
* @return True if succeeded
|
||||
*/
|
||||
bool getRobotPose(std::string path_frame_id, geometry_msgs::msg::PoseStamped & pose);
|
||||
|
||||
/**
|
||||
* @brief A custom pose distance method which takes angular distance into account
|
||||
* in addition to spatial distance (to improve picking a correct pose near cusps and loops)
|
||||
* @param pose1 Distance is computed between this pose and pose2
|
||||
* @param pose2 Distance is computed between this pose and pose1
|
||||
* @param angular_distance_weight Weight of angular distance relative to spatial distance
|
||||
* (1.0 means that 1 radian of angular distance corresponds to 1 meter of spatial distance)
|
||||
*/
|
||||
static double poseDistance(
|
||||
const geometry_msgs::msg::PoseStamped & pose1,
|
||||
const geometry_msgs::msg::PoseStamped & pose2,
|
||||
const double angular_distance_weight);
|
||||
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
|
||||
nav_msgs::msg::Path path_;
|
||||
nav_msgs::msg::Path::_poses_type::iterator closest_pose_detection_begin_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_LOCAL_ACTION_HPP_
|
||||
+63
@@ -0,0 +1,63 @@
|
||||
// Copyright (c) 2018 Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_ACTION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_ACTION_HPP_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "nav2_behavior_tree/bt_action_node.hpp"
|
||||
#include "nav2_msgs/action/wait.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait
|
||||
*/
|
||||
class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::WaitAction
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
WaitAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Function to perform some user-defined operation on tick
|
||||
*/
|
||||
void on_tick() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
BT::InputPort<int>("wait_duration", 1, "Wait time")
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_ACTION_HPP_
|
||||
+59
@@ -0,0 +1,59 @@
|
||||
// Copyright (c) 2022 Neobotix GmbH
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_CANCEL_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_CANCEL_NODE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "nav2_msgs/action/wait.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait
|
||||
*/
|
||||
class WaitCancel : public BtCancelActionNode<nav2_msgs::action::Wait>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::WaitAction
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param action_name Action name this node creates a client for
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
WaitCancel(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts(
|
||||
{
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_CANCEL_NODE_HPP_
|
||||
+82
@@ -0,0 +1,82 @@
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__DISTANCE_TRAVELED_CONDITION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__DISTANCE_TRAVELED_CONDITION_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "behaviortree_cpp_v3/condition_node.h"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "tf2_ros/buffer.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::ConditionNode that returns SUCCESS every time the robot
|
||||
* travels a specified distance and FAILURE otherwise
|
||||
*/
|
||||
class DistanceTraveledCondition : public BT::ConditionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::DistanceTraveledCondition
|
||||
* @param condition_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
DistanceTraveledCondition(
|
||||
const std::string & condition_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
DistanceTraveledCondition() = delete;
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<double>("distance", 1.0, "Distance"),
|
||||
BT::InputPort<std::string>("global_frame", std::string("map"), "Global frame"),
|
||||
BT::InputPort<std::string>("robot_base_frame", std::string("base_link"), "Robot base frame")
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_;
|
||||
|
||||
geometry_msgs::msg::PoseStamped start_pose_;
|
||||
|
||||
double distance_;
|
||||
double transform_tolerance_;
|
||||
std::string global_frame_;
|
||||
std::string robot_base_frame_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__DISTANCE_TRAVELED_CONDITION_HPP_
|
||||
+73
@@ -0,0 +1,73 @@
|
||||
// Copyright (c) 2021 Joshua Wallace
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include "behaviortree_cpp_v3/condition_node.h"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
/**
|
||||
* @brief A BT::ConditionNode that returns SUCCESS when goal is
|
||||
* updated on the blackboard and FAILURE otherwise
|
||||
*/
|
||||
class GloballyUpdatedGoalCondition : public BT::ConditionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::GloballyUpdatedGoalCondition
|
||||
* @param condition_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
GloballyUpdatedGoalCondition(
|
||||
const std::string & condition_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
GloballyUpdatedGoalCondition() = delete;
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {};
|
||||
}
|
||||
|
||||
private:
|
||||
bool first_time;
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
geometry_msgs::msg::PoseStamped goal_;
|
||||
std::vector<geometry_msgs::msg::PoseStamped> goals_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_
|
||||
+101
@@ -0,0 +1,101 @@
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_REACHED_CONDITION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_REACHED_CONDITION_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "behaviortree_cpp_v3/condition_node.h"
|
||||
#include "tf2_ros/buffer.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::ConditionNode that returns SUCCESS when a specified goal
|
||||
* is reached and FAILURE otherwise
|
||||
*/
|
||||
class GoalReachedCondition : public BT::ConditionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::GoalReachedCondition
|
||||
* @param condition_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
GoalReachedCondition(
|
||||
const std::string & condition_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
GoalReachedCondition() = delete;
|
||||
|
||||
/**
|
||||
* @brief A destructor for nav2_behavior_tree::GoalReachedCondition
|
||||
*/
|
||||
~GoalReachedCondition() override;
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief Function to read parameters and initialize class variables
|
||||
*/
|
||||
void initialize();
|
||||
|
||||
/**
|
||||
* @brief Checks if the current robot pose lies within a given distance from the goal
|
||||
* @return bool true when goal is reached, false otherwise
|
||||
*/
|
||||
bool isGoalReached();
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination"),
|
||||
BT::InputPort<std::string>("global_frame", std::string("map"), "Global frame"),
|
||||
BT::InputPort<std::string>("robot_base_frame", std::string("base_link"), "Robot base frame")
|
||||
};
|
||||
}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Cleanup function
|
||||
*/
|
||||
void cleanup()
|
||||
{}
|
||||
|
||||
private:
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_;
|
||||
|
||||
bool initialized_;
|
||||
double goal_reached_tol_;
|
||||
std::string global_frame_;
|
||||
std::string robot_base_frame_;
|
||||
double transform_tolerance_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_REACHED_CONDITION_HPP_
|
||||
+67
@@ -0,0 +1,67 @@
|
||||
// Copyright (c) 2020 Aitor Miguel Blanco
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_UPDATED_CONDITION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_UPDATED_CONDITION_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "behaviortree_cpp_v3/condition_node.h"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::ConditionNode that returns SUCCESS when goal is
|
||||
* updated on the blackboard and FAILURE otherwise
|
||||
*/
|
||||
class GoalUpdatedCondition : public BT::ConditionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::GoalUpdatedCondition
|
||||
* @param condition_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
GoalUpdatedCondition(
|
||||
const std::string & condition_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
GoalUpdatedCondition() = delete;
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {};
|
||||
}
|
||||
|
||||
private:
|
||||
geometry_msgs::msg::PoseStamped goal_;
|
||||
std::vector<geometry_msgs::msg::PoseStamped> goals_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_UPDATED_CONDITION_HPP_
|
||||
+29
@@ -0,0 +1,29 @@
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_
|
||||
|
||||
#include "behaviortree_cpp_v3/behavior_tree.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
/**
|
||||
* @brief A BT::ConditionNode that returns SUCCESS if initial pose
|
||||
* has been received and FAILURE otherwise
|
||||
*/
|
||||
BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node);
|
||||
}
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_
|
||||
+81
@@ -0,0 +1,81 @@
|
||||
// Copyright (c) 2023 Alberto J. Tudela Roldán
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_CHARGING_CONDITION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_CHARGING_CONDITION_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sensor_msgs/msg/battery_state.hpp"
|
||||
#include "behaviortree_cpp_v3/condition_node.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::ConditionNode that listens to a battery topic and
|
||||
* returns SUCCESS when battery is charging and FAILURE otherwise
|
||||
*/
|
||||
class IsBatteryChargingCondition : public BT::ConditionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::IsBatteryChargingCondition
|
||||
* @param condition_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
IsBatteryChargingCondition(
|
||||
const std::string & condition_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
IsBatteryChargingCondition() = delete;
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<std::string>(
|
||||
"battery_topic", std::string("/battery_status"), "Battery topic")
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Callback function for battery topic
|
||||
* @param msg Shared pointer to sensor_msgs::msg::BatteryState message
|
||||
*/
|
||||
void batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg);
|
||||
|
||||
rclcpp::CallbackGroup::SharedPtr callback_group_;
|
||||
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_sub_;
|
||||
std::string battery_topic_;
|
||||
bool is_battery_charging_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_CHARGING_CONDITION_HPP_
|
||||
+88
@@ -0,0 +1,88 @@
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_CONDITION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_CONDITION_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sensor_msgs/msg/battery_state.hpp"
|
||||
#include "behaviortree_cpp_v3/condition_node.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::ConditionNode that listens to a battery topic and
|
||||
* returns SUCCESS when battery is low and FAILURE otherwise
|
||||
*/
|
||||
class IsBatteryLowCondition : public BT::ConditionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::IsBatteryLowCondition
|
||||
* @param condition_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
IsBatteryLowCondition(
|
||||
const std::string & condition_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
IsBatteryLowCondition() = delete;
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<double>("min_battery", "Minimum battery percentage/voltage"),
|
||||
BT::InputPort<std::string>(
|
||||
"battery_topic", std::string("/battery_status"), "Battery topic"),
|
||||
BT::InputPort<bool>(
|
||||
"is_voltage", false, "If true voltage will be used to check for low battery"),
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Callback function for battery topic
|
||||
* @param msg Shared pointer to sensor_msgs::msg::BatteryState message
|
||||
*/
|
||||
void batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg);
|
||||
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
rclcpp::CallbackGroup::SharedPtr callback_group_;
|
||||
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_sub_;
|
||||
std::string battery_topic_;
|
||||
double min_battery_;
|
||||
bool is_voltage_;
|
||||
bool is_battery_low_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_CONDITION_HPP_
|
||||
+75
@@ -0,0 +1,75 @@
|
||||
// Copyright (c) 2021 Joshua Wallace
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_PATH_VALID_CONDITION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_PATH_VALID_CONDITION_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "behaviortree_cpp_v3/condition_node.h"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav2_msgs/srv/is_path_valid.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::ConditionNode that returns SUCCESS when the IsPathValid
|
||||
* service returns true and FAILURE otherwise
|
||||
*/
|
||||
class IsPathValidCondition : public BT::ConditionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::IsPathValidCondition
|
||||
* @param condition_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
IsPathValidCondition(
|
||||
const std::string & condition_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
IsPathValidCondition() = delete;
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<nav_msgs::msg::Path>("path", "Path to Check"),
|
||||
BT::InputPort<std::chrono::milliseconds>("server_timeout")
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
rclcpp::Client<nav2_msgs::srv::IsPathValid>::SharedPtr client_;
|
||||
// The timeout value while waiting for a responce from the
|
||||
// is path valid service
|
||||
std::chrono::milliseconds server_timeout_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_PATH_VALID_CONDITION_HPP_
|
||||
+110
@@ -0,0 +1,110 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STUCK_CONDITION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STUCK_CONDITION_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <atomic>
|
||||
#include <deque>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "behaviortree_cpp_v3/condition_node.h"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::ConditionNode that tracks robot odometry and returns SUCCESS
|
||||
* if robot is stuck somewhere and FAILURE otherwise
|
||||
*/
|
||||
class IsStuckCondition : public BT::ConditionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::IsStuckCondition
|
||||
* @param condition_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
IsStuckCondition(
|
||||
const std::string & condition_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
IsStuckCondition() = delete;
|
||||
|
||||
/**
|
||||
* @brief A destructor for nav2_behavior_tree::IsStuckCondition
|
||||
*/
|
||||
~IsStuckCondition() override;
|
||||
|
||||
/**
|
||||
* @brief Callback function for odom topic
|
||||
* @param msg Shared pointer to nav_msgs::msg::Odometry::SharedPtr message
|
||||
*/
|
||||
void onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg);
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief Function to log status when robot is stuck/free
|
||||
*/
|
||||
void logStuck(const std::string & msg) const;
|
||||
|
||||
/**
|
||||
* @brief Function to approximate acceleration from the odom history
|
||||
*/
|
||||
void updateStates();
|
||||
|
||||
/**
|
||||
* @brief Detect if robot bumped into something by checking for abnormal deceleration
|
||||
* @return bool true if robot is stuck, false otherwise
|
||||
*/
|
||||
bool isStuck();
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts() {return {};}
|
||||
|
||||
private:
|
||||
// The node that will be used for any ROS operations
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
rclcpp::CallbackGroup::SharedPtr callback_group_;
|
||||
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
|
||||
std::thread callback_group_executor_thread;
|
||||
|
||||
std::atomic<bool> is_stuck_;
|
||||
|
||||
// Listen to odometry
|
||||
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
||||
// Store history of odometry measurements
|
||||
std::deque<nav_msgs::msg::Odometry> odom_history_;
|
||||
std::deque<nav_msgs::msg::Odometry>::size_type odom_history_size_;
|
||||
|
||||
// Calculated states
|
||||
double current_accel_;
|
||||
|
||||
// Robot specific paramters
|
||||
double brake_accel_limit_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STUCK_CONDITION_HPP_
|
||||
+73
@@ -0,0 +1,73 @@
|
||||
// Copyright (c) 2022 Joshua Wallace
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__PATH_EXPIRING_TIMER_CONDITION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__PATH_EXPIRING_TIMER_CONDITION_HPP_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "behaviortree_cpp_v3/condition_node.h"
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::ConditionNode that returns SUCCESS every time a specified
|
||||
* time period passes and FAILURE otherwise
|
||||
*/
|
||||
class PathExpiringTimerCondition : public BT::ConditionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::PathExpiringTimerCondition
|
||||
* @param condition_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
PathExpiringTimerCondition(
|
||||
const std::string & condition_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
PathExpiringTimerCondition() = delete;
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<double>("seconds", 1.0, "Seconds"),
|
||||
BT::InputPort<nav_msgs::msg::Path>("path")
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
rclcpp::Time start_;
|
||||
nav_msgs::msg::Path prev_path_;
|
||||
double period_;
|
||||
bool first_time_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__PATH_EXPIRING_TIMER_CONDITION_HPP_
|
||||
+70
@@ -0,0 +1,70 @@
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TIME_EXPIRED_CONDITION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TIME_EXPIRED_CONDITION_HPP_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "behaviortree_cpp_v3/condition_node.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::ConditionNode that returns SUCCESS every time a specified
|
||||
* time period passes and FAILURE otherwise
|
||||
*/
|
||||
class TimeExpiredCondition : public BT::ConditionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::TimeExpiredCondition
|
||||
* @param condition_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
TimeExpiredCondition(
|
||||
const std::string & condition_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
TimeExpiredCondition() = delete;
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<double>("seconds", 1.0, "Seconds")
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
rclcpp::Time start_;
|
||||
double period_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TIME_EXPIRED_CONDITION_HPP_
|
||||
+82
@@ -0,0 +1,82 @@
|
||||
// Copyright (c) 2020 Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TRANSFORM_AVAILABLE_CONDITION_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TRANSFORM_AVAILABLE_CONDITION_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "behaviortree_cpp_v3/condition_node.h"
|
||||
#include "tf2_ros/buffer.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::ConditionNode that returns SUCCESS if there is a valid transform
|
||||
* between two specified frames and FAILURE otherwise
|
||||
*/
|
||||
class TransformAvailableCondition : public BT::ConditionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::TransformAvailableCondition
|
||||
* @param condition_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
TransformAvailableCondition(
|
||||
const std::string & condition_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
TransformAvailableCondition() = delete;
|
||||
|
||||
/**
|
||||
* @brief A destructor for nav2_behavior_tree::TransformAvailableCondition
|
||||
*/
|
||||
~TransformAvailableCondition();
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<std::string>("child", std::string(), "Child frame for transform"),
|
||||
BT::InputPort<std::string>("parent", std::string(), "parent frame for transform")
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_;
|
||||
|
||||
std::atomic<bool> was_found_;
|
||||
|
||||
std::string child_frame_;
|
||||
std::string parent_frame_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TRANSFORM_AVAILABLE_CONDITION_HPP_
|
||||
+93
@@ -0,0 +1,93 @@
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_
|
||||
|
||||
#include <string>
|
||||
#include "behaviortree_cpp_v3/control_node.h"
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/** @brief Type of sequence node that re-ticks previous children when a child returns running
|
||||
*
|
||||
* Type of Control Node | Child Returns Failure | Child Returns Running
|
||||
* ---------------------------------------------------------------------
|
||||
* PipelineSequence | Restart | Tick All Previous Again
|
||||
*
|
||||
* Tick All Previous Again means every node up till this one will be reticked. Even
|
||||
* if a previous node returns Running, the next node will be reticked.
|
||||
*
|
||||
* As an example, let's say this node has 3 children: A, B and C. At the start,
|
||||
* they are all IDLE.
|
||||
* | A | B | C |
|
||||
* --------------------------------
|
||||
* | IDLE | IDLE | IDLE |
|
||||
* | RUNNING | IDLE | IDLE | - at first A gets ticked. Assume it returns RUNNING
|
||||
* - PipelineSequence returns RUNNING and no other nodes are ticked.
|
||||
* | SUCCESS | RUNNING | IDLE | - This time A returns SUCCESS so B gets ticked as well
|
||||
* - PipelineSequence returns RUNNING and C is not ticked yet
|
||||
* | RUNNING | SUCCESS | RUNNING | - A gets ticked and returns RUNNING, but since it had previously
|
||||
* - returned SUCCESS, PipelineSequence continues on and ticks B.
|
||||
* - Since B also returns SUCCESS, C gets ticked this time as well.
|
||||
* | RUNNING | SUCCESS | SUCCESS | - A is still RUNNING, and B returns SUCCESS again. This time C
|
||||
* - returned SUCCESS, ending the sequence. PipelineSequence
|
||||
* - returns SUCCESS and halts A.
|
||||
*
|
||||
* If any children at any time had returned FAILURE. PipelineSequence would have returned FAILURE
|
||||
* and halted all children, ending the sequence.
|
||||
*
|
||||
* Usage in XML: <PipelineSequence>
|
||||
*/
|
||||
class PipelineSequence : public BT::ControlNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::PipelineSequence
|
||||
* @param name Name for the XML tag for this node
|
||||
*/
|
||||
explicit PipelineSequence(const std::string & name);
|
||||
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::PipelineSequence
|
||||
* @param name Name for the XML tag for this node
|
||||
* @param config BT node configuration
|
||||
*/
|
||||
PipelineSequence(const std::string & name, const BT::NodeConfiguration & config);
|
||||
|
||||
/**
|
||||
* @brief The other (optional) override required by a BT action to reset node state
|
||||
*/
|
||||
void halt() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts() {return {};}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
std::size_t last_child_ticked_ = 0;
|
||||
};
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_
|
||||
+82
@@ -0,0 +1,82 @@
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__RECOVERY_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__RECOVERY_NODE_HPP_
|
||||
|
||||
#include <string>
|
||||
#include "behaviortree_cpp_v3/control_node.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
/**
|
||||
* @brief The RecoveryNode has only two children and returns SUCCESS if and only if the first child
|
||||
* returns SUCCESS.
|
||||
*
|
||||
* - If the first child returns FAILURE, the second child will be executed. After that the first
|
||||
* child is executed again if the second child returns SUCCESS.
|
||||
*
|
||||
* - If the first or second child returns RUNNING, this node returns RUNNING.
|
||||
*
|
||||
* - If the second child returns FAILURE, this control node will stop the loop and returns FAILURE.
|
||||
*
|
||||
*/
|
||||
class RecoveryNode : public BT::ControlNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::RecoveryNode
|
||||
* @param name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
RecoveryNode(
|
||||
const std::string & name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief A destructor for nav2_behavior_tree::RecoveryNode
|
||||
*/
|
||||
~RecoveryNode() override = default;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<int>("number_of_retries", 1, "Number of retries")
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
unsigned int current_child_idx_;
|
||||
unsigned int number_of_retries_;
|
||||
unsigned int retry_count_;
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief The other (optional) override required by a BT action to reset node state
|
||||
*/
|
||||
void halt() override;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__RECOVERY_NODE_HPP_
|
||||
+94
@@ -0,0 +1,94 @@
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__ROUND_ROBIN_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__ROUND_ROBIN_NODE_HPP_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "behaviortree_cpp_v3/control_node.h"
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/** @brief Type of sequence node that ticks children in a round-robin fashion
|
||||
*
|
||||
* Type of Control Node | Child Returns Failure | Child Returns Running
|
||||
* ---------------------------------------------------------------------
|
||||
* RoundRobin | Tick Next Child | Return Running
|
||||
*
|
||||
* If the current child return failure, the next child is ticked and if the last child returns
|
||||
* failure, the first child is ticked and the cycle continues until a child returns success
|
||||
*
|
||||
* As an example, let's say this node has 3 children: A, B and C. At the start,
|
||||
* they are all IDLE.
|
||||
* | A | B | C |
|
||||
* --------------------------------
|
||||
* | IDLE | IDLE | IDLE |
|
||||
* | RUNNING | IDLE | IDLE | - at first A gets ticked. Assume it returns RUNNING
|
||||
* - RoundRobin returns RUNNING and no other nodes are ticked.
|
||||
* | FAILURE | RUNNING | IDLE | - A returns FAILURE so B gets ticked and returns RUNNING
|
||||
* - RoundRobin returns RUNNING and C is not ticked yet
|
||||
* | FAILURE | SUCCESS | IDLE | - B returns SUCCESS, so RoundRobin halts all children and
|
||||
* - returns SUCCESS, next iteration will tick C.
|
||||
* | RUNNING | IDLE | FAILURE | - C returns FAILURE, so RoundRobin circles and ticks A.
|
||||
* - A returns RUNNING, so RoundRobin returns RUNNING.
|
||||
*
|
||||
* If all children return FAILURE, RoundRobin will return FAILURE
|
||||
* and halt all children, ending the sequence.
|
||||
*
|
||||
* Usage in XML: <RoundRobin>
|
||||
*/
|
||||
class RoundRobinNode : public BT::ControlNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::RoundRobinNode
|
||||
* @param name Name for the XML tag for this node
|
||||
*/
|
||||
explicit RoundRobinNode(const std::string & name);
|
||||
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::RoundRobinNode
|
||||
* @param name Name for the XML tag for this node
|
||||
* @param config BT node configuration
|
||||
*/
|
||||
RoundRobinNode(const std::string & name, const BT::NodeConfiguration & config);
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief The other (optional) override required by a BT action to reset node state
|
||||
*/
|
||||
void halt() override;
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing basic ports along with node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts() {return {};}
|
||||
|
||||
private:
|
||||
unsigned int current_child_idx_{0};
|
||||
unsigned int num_failed_children_{0};
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__ROUND_ROBIN_NODE_HPP_
|
||||
+82
@@ -0,0 +1,82 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "tf2_ros/buffer.h"
|
||||
|
||||
#include "behaviortree_cpp_v3/decorator_node.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::DecoratorNode that ticks its child every time the robot
|
||||
* travels a specified distance
|
||||
*/
|
||||
class DistanceController : public BT::DecoratorNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::DistanceController
|
||||
* @param name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
DistanceController(
|
||||
const std::string & name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<double>("distance", 1.0, "Distance"),
|
||||
BT::InputPort<std::string>("global_frame", std::string("map"), "Global frame"),
|
||||
BT::InputPort<std::string>("robot_base_frame", std::string("base_link"), "Robot base frame")
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_;
|
||||
double transform_tolerance_;
|
||||
|
||||
geometry_msgs::msg::PoseStamped start_pose_;
|
||||
double distance_;
|
||||
|
||||
std::string global_frame_;
|
||||
std::string robot_base_frame_;
|
||||
|
||||
bool first_time_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_
|
||||
+68
@@ -0,0 +1,68 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_
|
||||
|
||||
#include <chrono>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "behaviortree_cpp_v3/decorator_node.h"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::DecoratorNode that ticks its child if the goal was updated
|
||||
*/
|
||||
class GoalUpdatedController : public BT::DecoratorNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::GoalUpdatedController
|
||||
* @param name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
GoalUpdatedController(
|
||||
const std::string & name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {};
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
bool goal_was_updated_;
|
||||
geometry_msgs::msg::PoseStamped goal_;
|
||||
std::vector<geometry_msgs::msg::PoseStamped> goals_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_
|
||||
+85
@@ -0,0 +1,85 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
// Copyright (c) 2020 Francisco Martin Rico
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
|
||||
#include "behaviortree_cpp_v3/decorator_node.h"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::DecoratorNode that subscribes to a goal topic and updates
|
||||
* the current goal on the blackboard
|
||||
*/
|
||||
class GoalUpdater : public BT::DecoratorNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::GoalUpdater
|
||||
* @param xml_tag_name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
GoalUpdater(
|
||||
const std::string & xml_tag_name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<geometry_msgs::msg::PoseStamped>("input_goal", "Original Goal"),
|
||||
BT::OutputPort<geometry_msgs::msg::PoseStamped>(
|
||||
"output_goal",
|
||||
"Received Goal by subscription"),
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief Callback function for goal update topic
|
||||
* @param msg Shared pointer to geometry_msgs::msg::PoseStamped message
|
||||
*/
|
||||
void callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
|
||||
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
|
||||
|
||||
geometry_msgs::msg::PoseStamped last_goal_received_;
|
||||
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
rclcpp::CallbackGroup::SharedPtr callback_group_;
|
||||
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_
|
||||
+113
@@ -0,0 +1,113 @@
|
||||
// Copyright (c) 2022 Neobotix GmbH
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__PATH_LONGER_ON_APPROACH_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__PATH_LONGER_ON_APPROACH_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <limits>
|
||||
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
#include "behaviortree_cpp_v3/decorator_node.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::DecoratorNode that ticks its child everytime when the length of
|
||||
* the new path is smaller than the old one by the length given by the user.
|
||||
*/
|
||||
class PathLongerOnApproach : public BT::DecoratorNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::PathLongerOnApproach
|
||||
* @param name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
PathLongerOnApproach(
|
||||
const std::string & name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<nav_msgs::msg::Path>("path", "Planned Path"),
|
||||
BT::InputPort<double>(
|
||||
"prox_len", 3.0,
|
||||
"Proximity length (m) for the path to be longer on approach"),
|
||||
BT::InputPort<double>(
|
||||
"length_factor", 2.0,
|
||||
"Length multiplication factor to check if the path is significantly longer"),
|
||||
};
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Checks if the global path is updated
|
||||
* @param new_path new path to the goal
|
||||
* @param old_path current path to the goal
|
||||
* @return whether the path is updated for the current goal
|
||||
*/
|
||||
bool isPathUpdated(
|
||||
nav_msgs::msg::Path & new_path,
|
||||
nav_msgs::msg::Path & old_path);
|
||||
|
||||
/**
|
||||
* @brief Checks if the robot is in the goal proximity
|
||||
* @param old_path current path to the goal
|
||||
* @param prox_leng proximity length from the goal
|
||||
* @return whether the robot is in the goal proximity
|
||||
*/
|
||||
bool isRobotInGoalProximity(
|
||||
nav_msgs::msg::Path & old_path,
|
||||
double & prox_leng);
|
||||
|
||||
/**
|
||||
* @brief Checks if the new path is longer
|
||||
* @param new_path new path to the goal
|
||||
* @param old_path current path to the goal
|
||||
* @param length_factor multipler for path length check
|
||||
* @return whether the new path is longer
|
||||
*/
|
||||
bool isNewPathLonger(
|
||||
nav_msgs::msg::Path & new_path,
|
||||
nav_msgs::msg::Path & old_path,
|
||||
double & length_factor);
|
||||
|
||||
private:
|
||||
nav_msgs::msg::Path new_path_;
|
||||
nav_msgs::msg::Path old_path_;
|
||||
double prox_len_ = std::numeric_limits<double>::max();
|
||||
double length_factor_ = std::numeric_limits<double>::max();
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
bool first_time_ = true;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__PATH_LONGER_ON_APPROACH_HPP_
|
||||
+66
@@ -0,0 +1,66 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__RATE_CONTROLLER_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__RATE_CONTROLLER_HPP_
|
||||
|
||||
#include <chrono>
|
||||
#include <string>
|
||||
|
||||
#include "behaviortree_cpp_v3/decorator_node.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::DecoratorNode that ticks its child at a specified rate
|
||||
*/
|
||||
class RateController : public BT::DecoratorNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::RateController
|
||||
* @param name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
RateController(
|
||||
const std::string & name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<double>("hz", 10.0, "Rate")
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
std::chrono::time_point<std::chrono::high_resolution_clock> start_;
|
||||
double period_;
|
||||
bool first_time_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__RATE_CONTROLLER_HPP_
|
||||
+63
@@ -0,0 +1,63 @@
|
||||
// Copyright (c) 2021 Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SINGLE_TRIGGER_NODE_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SINGLE_TRIGGER_NODE_HPP_
|
||||
|
||||
#include <chrono>
|
||||
#include <string>
|
||||
|
||||
#include "behaviortree_cpp_v3/decorator_node.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::DecoratorNode that triggers its child only once and returns FAILURE
|
||||
* for every succeeding tick
|
||||
*/
|
||||
class SingleTrigger : public BT::DecoratorNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::SingleTrigger
|
||||
* @param name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
SingleTrigger(
|
||||
const std::string & name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {};
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
bool first_time_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SINGLE_TRIGGER_NODE_HPP_
|
||||
+123
@@ -0,0 +1,123 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SPEED_CONTROLLER_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SPEED_CONTROLLER_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <deque>
|
||||
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "nav2_util/odometry_utils.hpp"
|
||||
|
||||
#include "behaviortree_cpp_v3/decorator_node.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A BT::DecoratorNode that ticks its child every at a rate proportional to
|
||||
* the speed of the robot. If the robot travels faster, this node will tick its child at a
|
||||
* higher frequency and reduce the tick frequency if the robot slows down
|
||||
*/
|
||||
class SpeedController : public BT::DecoratorNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::SpeedController
|
||||
* @param name Name for the XML tag for this node
|
||||
* @param conf BT node configuration
|
||||
*/
|
||||
SpeedController(
|
||||
const std::string & name,
|
||||
const BT::NodeConfiguration & conf);
|
||||
|
||||
/**
|
||||
* @brief Creates list of BT ports
|
||||
* @return BT::PortsList Containing node-specific ports
|
||||
*/
|
||||
static BT::PortsList providedPorts()
|
||||
{
|
||||
return {
|
||||
BT::InputPort<double>("min_rate", 0.1, "Minimum rate"),
|
||||
BT::InputPort<double>("max_rate", 1.0, "Maximum rate"),
|
||||
BT::InputPort<double>("min_speed", 0.0, "Minimum speed"),
|
||||
BT::InputPort<double>("max_speed", 0.5, "Maximum speed"),
|
||||
};
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief The main override required by a BT action
|
||||
* @return BT::NodeStatus Status of tick execution
|
||||
*/
|
||||
BT::NodeStatus tick() override;
|
||||
|
||||
/**
|
||||
* @brief Scale the rate based speed
|
||||
* @return double Rate scaled by speed limits and clamped
|
||||
*/
|
||||
inline double getScaledRate(const double & speed)
|
||||
{
|
||||
return std::max(
|
||||
std::min(
|
||||
(((speed - min_speed_) / d_speed_) * d_rate_) + min_rate_,
|
||||
max_rate_), min_rate_);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Update period based on current smoothed speed and reset timer
|
||||
*/
|
||||
inline void updatePeriod()
|
||||
{
|
||||
auto velocity = odom_smoother_->getTwist();
|
||||
double speed = std::hypot(velocity.linear.x, velocity.linear.y);
|
||||
double rate = getScaledRate(speed);
|
||||
period_ = 1.0 / rate;
|
||||
}
|
||||
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
|
||||
// To keep track of time to reset
|
||||
rclcpp::Time start_;
|
||||
|
||||
// To get a smoothed velocity
|
||||
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
|
||||
|
||||
bool first_tick_;
|
||||
|
||||
// Time period after which child node should be ticked
|
||||
double period_;
|
||||
|
||||
// Rates thresholds to tick child node
|
||||
double min_rate_;
|
||||
double max_rate_;
|
||||
double d_rate_;
|
||||
|
||||
// Speed thresholds
|
||||
double min_speed_;
|
||||
double max_speed_;
|
||||
double d_speed_;
|
||||
|
||||
// current goal
|
||||
geometry_msgs::msg::PoseStamped goal_;
|
||||
std::vector<geometry_msgs::msg::PoseStamped> goals_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SPEED_CONTROLLER_HPP_
|
||||
@@ -0,0 +1,107 @@
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_
|
||||
#define NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_
|
||||
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
|
||||
#include "behaviortree_cpp_v3/loggers/abstract_logger.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_msgs/msg/behavior_tree_log.hpp"
|
||||
#include "nav2_msgs/msg/behavior_tree_status_change.h"
|
||||
#include "tf2_ros/buffer_interface.h"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A class to publish BT logs on BT status change
|
||||
*/
|
||||
class RosTopicLogger : public BT::StatusChangeLogger
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_behavior_tree::RosTopicLogger
|
||||
* @param ros_node Weak pointer to parent rclcpp::Node
|
||||
* @param tree BT to monitor
|
||||
*/
|
||||
RosTopicLogger(const rclcpp::Node::WeakPtr & ros_node, const BT::Tree & tree)
|
||||
: StatusChangeLogger(tree.rootNode())
|
||||
{
|
||||
auto node = ros_node.lock();
|
||||
clock_ = node->get_clock();
|
||||
logger_ = node->get_logger();
|
||||
log_pub_ = node->create_publisher<nav2_msgs::msg::BehaviorTreeLog>(
|
||||
"behavior_tree_log",
|
||||
rclcpp::QoS(10));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Callback function which is called each time BT changes status
|
||||
* @param timestamp Timestamp of BT status change
|
||||
* @param node Node that changed status
|
||||
* @param prev_status Previous status of the node
|
||||
* @param status Current status of the node
|
||||
*/
|
||||
void callback(
|
||||
BT::Duration timestamp,
|
||||
const BT::TreeNode & node,
|
||||
BT::NodeStatus prev_status,
|
||||
BT::NodeStatus status) override
|
||||
{
|
||||
nav2_msgs::msg::BehaviorTreeStatusChange event;
|
||||
|
||||
// BT timestamps are a duration since the epoch. Need to convert to a time_point
|
||||
// before converting to a msg.
|
||||
event.timestamp = tf2_ros::toMsg(tf2::TimePoint(timestamp));
|
||||
event.node_name = node.name();
|
||||
event.previous_status = toStr(prev_status, false);
|
||||
event.current_status = toStr(status, false);
|
||||
event_log_.push_back(std::move(event));
|
||||
|
||||
RCLCPP_DEBUG(
|
||||
logger_, "[%.3f]: %25s %s -> %s",
|
||||
std::chrono::duration<double>(timestamp).count(),
|
||||
node.name().c_str(),
|
||||
toStr(prev_status, true).c_str(),
|
||||
toStr(status, true).c_str() );
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Clear log buffer if any
|
||||
*/
|
||||
void flush() override
|
||||
{
|
||||
if (!event_log_.empty()) {
|
||||
auto log_msg = std::make_unique<nav2_msgs::msg::BehaviorTreeLog>();
|
||||
log_msg->timestamp = clock_->now();
|
||||
log_msg->event_log = event_log_;
|
||||
log_pub_->publish(std::move(log_msg));
|
||||
event_log_.clear();
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
rclcpp::Clock::SharedPtr clock_;
|
||||
rclcpp::Logger logger_{rclcpp::get_logger("bt_navigator")};
|
||||
rclcpp::Publisher<nav2_msgs::msg::BehaviorTreeLog>::SharedPtr log_pub_;
|
||||
std::vector<nav2_msgs::msg::BehaviorTreeStatusChange> event_log_;
|
||||
};
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#endif // NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_
|
||||
@@ -0,0 +1,300 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
For instructions on using Groot and description of the following BehaviorTree nodes,
|
||||
please refer to the groot_instructions.md and REAMDE.md respectively located in the
|
||||
nav2_behavior_tree package.
|
||||
-->
|
||||
<root>
|
||||
<TreeNodesModel>
|
||||
<!-- ############################### ACTION NODES ################################# -->
|
||||
<Action ID="BackUp">
|
||||
<input_port name="backup_dist">Distance to backup</input_port>
|
||||
<input_port name="backup_speed">Speed at which to backup</input_port>
|
||||
<input_port name="time_allowance">Allowed time for reversing</input_port>
|
||||
<input_port name="server_name">Server name</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="DriveOnHeading">
|
||||
<input_port name="dist_to_travel">Distance to travel</input_port>
|
||||
<input_port name="speed">Speed at which to travel</input_port>
|
||||
<input_port name="time_allowance">Allowed time for reversing</input_port>
|
||||
<input_port name="server_name">Server name</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="CancelControl">
|
||||
<input_port name="server_name">Server name to cancel the controller server</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="CancelBackUp">
|
||||
<input_port name="server_name">Server name to cancel the backup behavior</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="CancelDriveOnHeading">
|
||||
<input_port name="server_name">Service name to cancel the drive on heading behavior</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="CancelSpin">
|
||||
<input_port name="server_name">Server name to cancel the spin behavior</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="CancelAssistedTeleop">
|
||||
<input_port name="server_name">Server name to cancel the assisted teleop behavior</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="CancelWait">
|
||||
<input_port name="server_name">Server name to cancel the wait behavior</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="ClearEntireCostmap">
|
||||
<input_port name="service_name">Service name</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="ClearCostmapExceptRegion">
|
||||
<input_port name="service_name">Service name</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
<input_port name="reset_distance">Distance from the robot above which obstacles are cleared</input_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="ClearCostmapAroundRobot">
|
||||
<input_port name="service_name">Service name</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
<input_port name="reset_distance">Distance from the robot under which obstacles are cleared</input_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="ComputePathToPose">
|
||||
<input_port name="goal">Destination to plan to</input_port>
|
||||
<input_port name="start">Start pose of the path if overriding current robot pose</input_port>
|
||||
<input_port name="planner_id">Mapped name to the planner plugin type to use</input_port>
|
||||
<input_port name="server_name">Server name</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
<output_port name="path">Path created by ComputePathToPose node</output_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="ComputePathThroughPoses">
|
||||
<input_port name="goals">Destinations to plan through</input_port>
|
||||
<input_port name="start">Start pose of the path if overriding current robot pose</input_port>
|
||||
<input_port name="server_name">Server name</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
<input_port name="planner_id">Mapped name to the planner plugin type to use</input_port>
|
||||
<output_port name="path">Path created by ComputePathToPose node</output_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="RemovePassedGoals">
|
||||
<input_port name="input_goals">Input goals to remove if passed</input_port>
|
||||
<input_port name="radius">Radius tolerance on a goal to consider it passed</input_port>
|
||||
<input_port name="global_frame">Global frame</input_port>
|
||||
<input_port name="robot_base_frame">Robot base frame</input_port>
|
||||
<output_port name="output_goals">Set of goals after removing any passed</output_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="SmoothPath">
|
||||
<input_port name="smoother_id" default="SmoothPath"/>
|
||||
<input_port name="unsmoothed_path">Path to be smoothed</input_port>
|
||||
<input_port name="max_smoothing_duration">Maximum smoothing duration</input_port>
|
||||
<input_port name="check_for_collisions">Bool if collision check should be performed</input_port>
|
||||
<output_port name="smoothed_path">Smoothed path</output_port>
|
||||
<output_port name="smoothing_duration">Smoothing duration</output_port>
|
||||
<output_port name="was_completed">True if smoothing was not interrupted by time limit</output_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="FollowPath">
|
||||
<input_port name="controller_id" default="FollowPath"/>
|
||||
<input_port name="path">Path to follow</input_port>
|
||||
<input_port name="goal_checker_id">Goal checker</input_port>
|
||||
<input_port name="server_name">Server name</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="NavigateToPose">
|
||||
<input_port name="goal">Goal</input_port>
|
||||
<input_port name="server_name">Server name</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
<input_port name="behavior_tree">Behavior tree to run</input_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="NavigateThroughPoses">
|
||||
<input_port name="goals">Goals</input_port>
|
||||
<input_port name="server_name">Server name</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
<input_port name="behavior_tree">Behavior tree to run</input_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="ReinitializeGlobalLocalization">
|
||||
<input_port name="service_name">Service name</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="TruncatePath">
|
||||
<input_port name="distance">Distance before goal to truncate</input_port>
|
||||
<input_port name="input_path">Path to truncate</input_port>
|
||||
<output_port name="output_path">Truncated path to utilize</output_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="TruncatePathLocal">
|
||||
<input_port name="distance_forward">Distance in forward direction</input_port>
|
||||
<input_port name="distance_backward">Distance in backward direction</input_port>
|
||||
<input_port name="robot_frame">Robot base frame id</input_port>
|
||||
<input_port name="transform_tolerance">Transform lookup tolerance</input_port>
|
||||
<input_port name="pose">Manually specified pose to be used if overriding current robot pose</input_port>
|
||||
<input_port name="angular_distance_weight">Weight of angular distance relative to positional distance when finding which path pose is closest to robot. Not applicable on paths without orientations assigned</input_port>
|
||||
<input_port name="max_robot_pose_search_dist">Maximum forward integrated distance along the path (starting from the last detected pose) to bound the search for the closest pose to the robot. When set to infinity (default), whole path is searched every time</input_port>
|
||||
<output_port name="output_path">Truncated path to utilize</output_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="PlannerSelector">
|
||||
<input_port name="topic_name">Name of the topic to receive planner selection commands</input_port>
|
||||
<input_port name="default_planner">Default planner of the planner selector</input_port>
|
||||
<output_port name="selected_planner">Name of the selected planner received from the topic subcription</output_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="ControllerSelector">
|
||||
<input_port name="topic_name">Name of the topic to receive controller selection commands</input_port>
|
||||
<input_port name="default_controller">Default controller of the controller selector</input_port>
|
||||
<output_port name="selected_controller">Name of the selected controller received from the topic subcription</output_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="SmootherSelector">
|
||||
<input_port name="topic_name">Name of the topic to receive smoother selection commands</input_port>
|
||||
<input_port name="default_smoother">Default smoother of the smoother selector</input_port>
|
||||
<output_port name="selected_smoother">Name of the selected smoother received from the topic subcription</output_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="GoalCheckerSelector">
|
||||
<input_port name="topic_name">Name of the topic to receive goal checker selection commands</input_port>
|
||||
<input_port name="default_goal_checker">Default goal checker of the controller selector</input_port>
|
||||
<output_port name="selected_goal_checker">Name of the selected goal checker received from the topic subcription</output_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="ProgressCheckerSelector">
|
||||
<input_port name="topic_name">Name of the topic to receive progress checker selection commands</input_port>
|
||||
<input_port name="default_progress_checker">Default progress checker of the controller selector</input_port>
|
||||
<output_port name="selected_progress_checker">Name of the selected progress checker received from the topic subcription</output_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="Spin">
|
||||
<input_port name="spin_dist">Spin distance</input_port>
|
||||
<input_port name="time_allowance">Allowed time for spinning</input_port>
|
||||
<input_port name="server_name">Server name</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="Wait">
|
||||
<input_port name="wait_duration">Wait time</input_port>
|
||||
<input_port name="server_name">Server name</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
</Action>
|
||||
|
||||
<Action ID="AssistedTeleop">
|
||||
<input_port name="time_allowance">Allowed time for spinning</input_port>
|
||||
<input_port name="is_recovery">If true recovery count will be incremented</input_port>
|
||||
<input_port name="server_name">Service name</input_port>
|
||||
<input_port name="server_timeout">Server timeout</input_port>
|
||||
</Action>
|
||||
|
||||
<!-- ############################### CONDITION NODES ############################## -->
|
||||
<Condition ID="GoalReached">
|
||||
<input_port name="goal">Destination</input_port>
|
||||
<input_port name="global_frame">Reference frame</input_port>
|
||||
<input_port name="robot_base_frame">Robot base frame</input_port>
|
||||
</Condition>
|
||||
|
||||
<Condition ID="IsStuck"/>
|
||||
|
||||
<Condition ID="TransformAvailable">
|
||||
<input_port name="child">Child frame for transform</input_port>
|
||||
<input_port name="parent">Parent frame for transform</input_port>
|
||||
</Condition>
|
||||
|
||||
<Condition ID="GoalUpdated"/>
|
||||
|
||||
<Condition ID="GlobalUpdatedGoal"/>
|
||||
|
||||
<Condition ID="IsBatteryLow">
|
||||
<input_port name="min_battery">Min battery % or voltage before triggering</input_port>
|
||||
<input_port name="battery_topic">Topic for battery info</input_port>
|
||||
<input_port name="is_voltage">Bool if check based on voltage or total %</input_port>
|
||||
</Condition>
|
||||
|
||||
<Condition ID="IsBatteryCharging">
|
||||
<input_port name="battery_topic">Topic for battery info</input_port>
|
||||
</Condition>
|
||||
|
||||
<Condition ID="DistanceTraveled">
|
||||
<input_port name="distance">Distance to check if passed</input_port>
|
||||
<input_port name="global_frame">reference frame to check in</input_port>
|
||||
<input_port name="robot_base_frame">Robot frame to check relative to global_frame</input_port>
|
||||
</Condition>
|
||||
|
||||
<Condition ID="TimeExpired">
|
||||
<input_port name="seconds">Time to check if expired</input_port>
|
||||
</Condition>
|
||||
|
||||
|
||||
<Condition ID="PathExpiringTimer">
|
||||
<input_port name="seconds">Time to check if expired</input_port>
|
||||
<input_port name="path">Check if path has been updated to enable timer reset</input_port>
|
||||
</Condition>
|
||||
|
||||
<Condition ID="InitialPoseReceived">
|
||||
</Condition>
|
||||
|
||||
<Condition ID="IsPathValid">
|
||||
<input_port name="path"> Path to validate </input_port>
|
||||
<input_port name="server_timeout"> Server timeout </input_port>
|
||||
</Condition>
|
||||
|
||||
<!-- ############################### CONTROL NODES ################################ -->
|
||||
<Control ID="PipelineSequence"/>
|
||||
|
||||
<Control ID="RecoveryNode">
|
||||
<input_port name="number_of_retries">Number of retries</input_port>
|
||||
</Control>
|
||||
|
||||
<Control ID="RoundRobin"/>
|
||||
|
||||
<!-- ############################### DECORATOR NODES ############################## -->
|
||||
<Decorator ID="RateController">
|
||||
<input_port name="hz">Rate</input_port>
|
||||
</Decorator>
|
||||
|
||||
<Decorator ID="DistanceController">
|
||||
<input_port name="distance">Distance</input_port>
|
||||
<input_port name="global_frame">Reference frame</input_port>
|
||||
<input_port name="robot_base_frame">Robot base frame</input_port>
|
||||
</Decorator>
|
||||
|
||||
<Decorator ID="SingleTrigger">
|
||||
</Decorator>
|
||||
|
||||
<Decorator ID="GoalUpdater">
|
||||
<input_port name="input_goal">Original goal in</input_port>
|
||||
<output_port name="output_goal">Output goal set by subscription</output_port>
|
||||
</Decorator>
|
||||
|
||||
<Decorator ID="SpeedController">
|
||||
<input_port name="min_rate">Minimum rate</input_port>
|
||||
<input_port name="max_rate">Maximum rate</input_port>
|
||||
<input_port name="min_speed">Minimum speed</input_port>
|
||||
<input_port name="max_speed">Maximum speed</input_port>
|
||||
</Decorator>
|
||||
|
||||
<Decorator ID="PathLongerOnApproach">
|
||||
<input_port name="path">Planned Path</input_port>
|
||||
<input_port name="prox_len">Proximity length (m) for the path to be longer on approach</input_port>
|
||||
<input_port name="length_factor">Length multiplication factor to check if the path is significantly longer </input_port>
|
||||
</Decorator>
|
||||
|
||||
<Decorator ID="GoalUpdatedController">
|
||||
</Decorator>
|
||||
|
||||
</TreeNodesModel>
|
||||
</root>
|
||||
@@ -0,0 +1,59 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>nav2_behavior_tree</name>
|
||||
<version>1.1.18</version>
|
||||
<description>TODO</description>
|
||||
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
|
||||
<maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
|
||||
<maintainer email="mohammad.haghighipanah@intel.com">Carlos Orduno</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<build_export_depend>tf2_geometry_msgs</build_export_depend>
|
||||
<build_export_depend>std_srvs</build_export_depend>
|
||||
|
||||
<build_depend>rclcpp</build_depend>
|
||||
<build_depend>rclcpp_action</build_depend>
|
||||
<build_depend>rclcpp_lifecycle</build_depend>
|
||||
<build_depend>behaviortree_cpp_v3</build_depend>
|
||||
<build_depend>builtin_interfaces</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>nav2_msgs</build_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>tf2</build_depend>
|
||||
<build_depend>tf2_ros</build_depend>
|
||||
<build_depend>tf2_geometry_msgs</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>std_srvs</build_depend>
|
||||
<build_depend>nav2_util</build_depend>
|
||||
<build_depend>lifecycle_msgs</build_depend>
|
||||
<build_depend>nav2_common</build_depend>
|
||||
|
||||
<exec_depend>rclcpp</exec_depend>
|
||||
<exec_depend>rclcpp_action</exec_depend>
|
||||
<exec_depend>rclcpp_lifecycle</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>behaviortree_cpp_v3</exec_depend>
|
||||
<exec_depend>builtin_interfaces</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>nav2_msgs</exec_depend>
|
||||
<exec_depend>nav_msgs</exec_depend>
|
||||
<exec_depend>tf2</exec_depend>
|
||||
<exec_depend>tf2_ros</exec_depend>
|
||||
<exec_depend>tf2_geometry_msgs</exec_depend>
|
||||
<exec_depend>nav2_util</exec_depend>
|
||||
<exec_depend>lifecycle_msgs</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
<test_depend>test_msgs</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,62 @@
|
||||
// Copyright (c) 2022 Joshua Wallace
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
AssistedTeleopAction::AssistedTeleopAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtActionNode<nav2_msgs::action::AssistedTeleop>(xml_tag_name, action_name, conf)
|
||||
{
|
||||
double time_allowance;
|
||||
getInput("time_allowance", time_allowance);
|
||||
getInput("is_recovery", is_recovery_);
|
||||
|
||||
// Populate the input message
|
||||
goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
|
||||
}
|
||||
|
||||
void AssistedTeleopAction::on_tick()
|
||||
{
|
||||
if (is_recovery_) {
|
||||
increment_recovery_count();
|
||||
}
|
||||
}
|
||||
|
||||
BT::NodeStatus AssistedTeleopAction::on_aborted()
|
||||
{
|
||||
return is_recovery_ ? BT::NodeStatus::FAILURE : BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
BT::NodeBuilder builder =
|
||||
[](const std::string & name, const BT::NodeConfiguration & config)
|
||||
{
|
||||
return std::make_unique<nav2_behavior_tree::AssistedTeleopAction>(
|
||||
name, "assisted_teleop", config);
|
||||
};
|
||||
|
||||
factory.registerBuilder<nav2_behavior_tree::AssistedTeleopAction>("AssistedTeleop", builder);
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
// Copyright (c) 2022 Joshua Wallace
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
AssistedTeleopCancel::AssistedTeleopCancel(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtCancelActionNode<nav2_msgs::action::AssistedTeleop>(xml_tag_name, action_name, conf)
|
||||
{
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
BT::NodeBuilder builder =
|
||||
[](const std::string & name, const BT::NodeConfiguration & config)
|
||||
{
|
||||
return std::make_unique<nav2_behavior_tree::AssistedTeleopCancel>(
|
||||
name, "assisted_teleop", config);
|
||||
};
|
||||
|
||||
factory.registerBuilder<nav2_behavior_tree::AssistedTeleopCancel>(
|
||||
"CancelAssistedTeleop", builder);
|
||||
}
|
||||
@@ -0,0 +1,62 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/back_up_action.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
BackUpAction::BackUpAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtActionNode<nav2_msgs::action::BackUp>(xml_tag_name, action_name, conf)
|
||||
{
|
||||
double dist;
|
||||
getInput("backup_dist", dist);
|
||||
double speed;
|
||||
getInput("backup_speed", speed);
|
||||
double time_allowance;
|
||||
getInput("time_allowance", time_allowance);
|
||||
|
||||
// Populate the input message
|
||||
goal_.target.x = dist;
|
||||
goal_.target.y = 0.0;
|
||||
goal_.target.z = 0.0;
|
||||
goal_.speed = speed;
|
||||
goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
|
||||
}
|
||||
|
||||
void BackUpAction::on_tick()
|
||||
{
|
||||
increment_recovery_count();
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
BT::NodeBuilder builder =
|
||||
[](const std::string & name, const BT::NodeConfiguration & config)
|
||||
{
|
||||
return std::make_unique<nav2_behavior_tree::BackUpAction>(
|
||||
name, "backup", config);
|
||||
};
|
||||
|
||||
factory.registerBuilder<nav2_behavior_tree::BackUpAction>("BackUp", builder);
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
// Copyright (c) 2022 Neobotix GmbH
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
BackUpCancel::BackUpCancel(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtCancelActionNode<nav2_msgs::action::BackUp>(xml_tag_name, action_name, conf)
|
||||
{
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
BT::NodeBuilder builder =
|
||||
[](const std::string & name, const BT::NodeConfiguration & config)
|
||||
{
|
||||
return std::make_unique<nav2_behavior_tree::BackUpCancel>(
|
||||
name, "backup", config);
|
||||
};
|
||||
|
||||
factory.registerBuilder<nav2_behavior_tree::BackUpCancel>(
|
||||
"CancelBackUp", builder);
|
||||
}
|
||||
@@ -0,0 +1,71 @@
|
||||
// Copyright (c) 2019 Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
ClearEntireCostmapService::ClearEntireCostmapService(
|
||||
const std::string & service_node_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>(service_node_name, conf)
|
||||
{
|
||||
}
|
||||
|
||||
void ClearEntireCostmapService::on_tick()
|
||||
{
|
||||
increment_recovery_count();
|
||||
}
|
||||
|
||||
ClearCostmapExceptRegionService::ClearCostmapExceptRegionService(
|
||||
const std::string & service_node_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtServiceNode<nav2_msgs::srv::ClearCostmapExceptRegion>(service_node_name, conf)
|
||||
{
|
||||
}
|
||||
|
||||
void ClearCostmapExceptRegionService::on_tick()
|
||||
{
|
||||
getInput("reset_distance", request_->reset_distance);
|
||||
increment_recovery_count();
|
||||
}
|
||||
|
||||
ClearCostmapAroundRobotService::ClearCostmapAroundRobotService(
|
||||
const std::string & service_node_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtServiceNode<nav2_msgs::srv::ClearCostmapAroundRobot>(service_node_name, conf)
|
||||
{
|
||||
}
|
||||
|
||||
void ClearCostmapAroundRobotService::on_tick()
|
||||
{
|
||||
getInput("reset_distance", request_->reset_distance);
|
||||
increment_recovery_count();
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
factory.registerNodeType<nav2_behavior_tree::ClearEntireCostmapService>("ClearEntireCostmap");
|
||||
factory.registerNodeType<nav2_behavior_tree::ClearCostmapExceptRegionService>(
|
||||
"ClearCostmapExceptRegion");
|
||||
factory.registerNodeType<nav2_behavior_tree::ClearCostmapAroundRobotService>(
|
||||
"ClearCostmapAroundRobot");
|
||||
}
|
||||
@@ -0,0 +1,75 @@
|
||||
// Copyright (c) 2021 Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
ComputePathThroughPosesAction::ComputePathThroughPosesAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtActionNode<nav2_msgs::action::ComputePathThroughPoses>(xml_tag_name, action_name, conf)
|
||||
{
|
||||
}
|
||||
|
||||
void ComputePathThroughPosesAction::on_tick()
|
||||
{
|
||||
getInput("goals", goal_.goals);
|
||||
getInput("planner_id", goal_.planner_id);
|
||||
if (getInput("start", goal_.start)) {
|
||||
goal_.use_start = true;
|
||||
}
|
||||
}
|
||||
|
||||
BT::NodeStatus ComputePathThroughPosesAction::on_success()
|
||||
{
|
||||
setOutput("path", result_.result->path);
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
BT::NodeStatus ComputePathThroughPosesAction::on_aborted()
|
||||
{
|
||||
nav_msgs::msg::Path empty_path;
|
||||
setOutput("path", empty_path);
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
BT::NodeStatus ComputePathThroughPosesAction::on_cancelled()
|
||||
{
|
||||
nav_msgs::msg::Path empty_path;
|
||||
setOutput("path", empty_path);
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
BT::NodeBuilder builder =
|
||||
[](const std::string & name, const BT::NodeConfiguration & config)
|
||||
{
|
||||
return std::make_unique<nav2_behavior_tree::ComputePathThroughPosesAction>(
|
||||
name, "compute_path_through_poses", config);
|
||||
};
|
||||
|
||||
factory.registerBuilder<nav2_behavior_tree::ComputePathThroughPosesAction>(
|
||||
"ComputePathThroughPoses", builder);
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
ComputePathToPoseAction::ComputePathToPoseAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtActionNode<nav2_msgs::action::ComputePathToPose>(xml_tag_name, action_name, conf)
|
||||
{
|
||||
}
|
||||
|
||||
void ComputePathToPoseAction::on_tick()
|
||||
{
|
||||
getInput("goal", goal_.goal);
|
||||
getInput("planner_id", goal_.planner_id);
|
||||
if (getInput("start", goal_.start)) {
|
||||
goal_.use_start = true;
|
||||
}
|
||||
}
|
||||
|
||||
BT::NodeStatus ComputePathToPoseAction::on_success()
|
||||
{
|
||||
setOutput("path", result_.result->path);
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
BT::NodeStatus ComputePathToPoseAction::on_aborted()
|
||||
{
|
||||
nav_msgs::msg::Path empty_path;
|
||||
setOutput("path", empty_path);
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
BT::NodeStatus ComputePathToPoseAction::on_cancelled()
|
||||
{
|
||||
nav_msgs::msg::Path empty_path;
|
||||
setOutput("path", empty_path);
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
void ComputePathToPoseAction::halt()
|
||||
{
|
||||
nav_msgs::msg::Path empty_path;
|
||||
setOutput("path", empty_path);
|
||||
BtActionNode::halt();
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
BT::NodeBuilder builder =
|
||||
[](const std::string & name, const BT::NodeConfiguration & config)
|
||||
{
|
||||
return std::make_unique<nav2_behavior_tree::ComputePathToPoseAction>(
|
||||
name, "compute_path_to_pose", config);
|
||||
};
|
||||
|
||||
factory.registerBuilder<nav2_behavior_tree::ComputePathToPoseAction>(
|
||||
"ComputePathToPose", builder);
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
// Copyright (c) 2022 Neobotix GmbH
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/controller_cancel_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
ControllerCancel::ControllerCancel(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtCancelActionNode<nav2_msgs::action::FollowPath>(xml_tag_name, action_name, conf)
|
||||
{
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
BT::NodeBuilder builder =
|
||||
[](const std::string & name, const BT::NodeConfiguration & config)
|
||||
{
|
||||
return std::make_unique<nav2_behavior_tree::ControllerCancel>(
|
||||
name, "follow_path", config);
|
||||
};
|
||||
|
||||
factory.registerBuilder<nav2_behavior_tree::ControllerCancel>(
|
||||
"CancelControl", builder);
|
||||
}
|
||||
@@ -0,0 +1,91 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
// Copyright (c) 2020 Pablo Iñigo Blasco
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/controller_selector_node.hpp"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
using std::placeholders::_1;
|
||||
|
||||
ControllerSelector::ControllerSelector(
|
||||
const std::string & name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BT::SyncActionNode(name, conf)
|
||||
{
|
||||
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
|
||||
callback_group_ = node_->create_callback_group(
|
||||
rclcpp::CallbackGroupType::MutuallyExclusive,
|
||||
false);
|
||||
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
|
||||
|
||||
getInput("topic_name", topic_name_);
|
||||
|
||||
rclcpp::QoS qos(rclcpp::KeepLast(1));
|
||||
qos.transient_local().reliable();
|
||||
|
||||
rclcpp::SubscriptionOptions sub_option;
|
||||
sub_option.callback_group = callback_group_;
|
||||
controller_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
|
||||
topic_name_,
|
||||
qos,
|
||||
std::bind(&ControllerSelector::callbackControllerSelect, this, _1),
|
||||
sub_option);
|
||||
}
|
||||
|
||||
BT::NodeStatus ControllerSelector::tick()
|
||||
{
|
||||
callback_group_executor_.spin_some();
|
||||
|
||||
// This behavior always use the last selected controller received from the topic input.
|
||||
// When no input is specified it uses the default controller.
|
||||
// If the default controller is not specified then we work in "required controller mode":
|
||||
// In this mode, the behavior returns failure if the controller selection is not received from
|
||||
// the topic input.
|
||||
if (last_selected_controller_.empty()) {
|
||||
std::string default_controller;
|
||||
getInput("default_controller", default_controller);
|
||||
if (default_controller.empty()) {
|
||||
return BT::NodeStatus::FAILURE;
|
||||
} else {
|
||||
last_selected_controller_ = default_controller;
|
||||
}
|
||||
}
|
||||
|
||||
setOutput("selected_controller", last_selected_controller_);
|
||||
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
void
|
||||
ControllerSelector::callbackControllerSelect(const std_msgs::msg::String::SharedPtr msg)
|
||||
{
|
||||
last_selected_controller_ = msg->data;
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
factory.registerNodeType<nav2_behavior_tree::ControllerSelector>("ControllerSelector");
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
DriveOnHeadingAction::DriveOnHeadingAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtActionNode<nav2_msgs::action::DriveOnHeading>(xml_tag_name, action_name, conf)
|
||||
{
|
||||
double dist;
|
||||
getInput("dist_to_travel", dist);
|
||||
double speed;
|
||||
getInput("speed", speed);
|
||||
double time_allowance;
|
||||
getInput("time_allowance", time_allowance);
|
||||
|
||||
// Populate the input message
|
||||
goal_.target.x = dist;
|
||||
goal_.target.y = 0.0;
|
||||
goal_.target.z = 0.0;
|
||||
goal_.speed = speed;
|
||||
goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
BT::NodeBuilder builder =
|
||||
[](const std::string & name, const BT::NodeConfiguration & config)
|
||||
{
|
||||
return std::make_unique<nav2_behavior_tree::DriveOnHeadingAction>(
|
||||
name, "drive_on_heading", config);
|
||||
};
|
||||
|
||||
factory.registerBuilder<nav2_behavior_tree::DriveOnHeadingAction>("DriveOnHeading", builder);
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
// Copyright (c) 2022 Neobotix GmbH
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
DriveOnHeadingCancel::DriveOnHeadingCancel(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtCancelActionNode<nav2_msgs::action::DriveOnHeading>(xml_tag_name, action_name, conf)
|
||||
{
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
BT::NodeBuilder builder =
|
||||
[](const std::string & name, const BT::NodeConfiguration & config)
|
||||
{
|
||||
return std::make_unique<nav2_behavior_tree::DriveOnHeadingCancel>(
|
||||
name, "drive_on_heading", config);
|
||||
};
|
||||
|
||||
factory.registerBuilder<nav2_behavior_tree::DriveOnHeadingCancel>(
|
||||
"CancelDriveOnHeading", builder);
|
||||
}
|
||||
@@ -0,0 +1,83 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/follow_path_action.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
FollowPathAction::FollowPathAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtActionNode<nav2_msgs::action::FollowPath>(xml_tag_name, action_name, conf)
|
||||
{
|
||||
}
|
||||
|
||||
void FollowPathAction::on_tick()
|
||||
{
|
||||
getInput("path", goal_.path);
|
||||
getInput("controller_id", goal_.controller_id);
|
||||
getInput("goal_checker_id", goal_.goal_checker_id);
|
||||
}
|
||||
|
||||
void FollowPathAction::on_wait_for_result(
|
||||
std::shared_ptr<const nav2_msgs::action::FollowPath::Feedback>/*feedback*/)
|
||||
{
|
||||
// Grab the new path
|
||||
nav_msgs::msg::Path new_path;
|
||||
getInput("path", new_path);
|
||||
|
||||
// Check if it is not same with the current one
|
||||
if (goal_.path != new_path && new_path != nav_msgs::msg::Path()) {
|
||||
// the action server on the next loop iteration
|
||||
goal_.path = new_path;
|
||||
goal_updated_ = true;
|
||||
}
|
||||
|
||||
std::string new_controller_id;
|
||||
getInput("controller_id", new_controller_id);
|
||||
|
||||
if (goal_.controller_id != new_controller_id) {
|
||||
goal_.controller_id = new_controller_id;
|
||||
goal_updated_ = true;
|
||||
}
|
||||
|
||||
std::string new_goal_checker_id;
|
||||
getInput("goal_checker_id", new_goal_checker_id);
|
||||
|
||||
if (goal_.goal_checker_id != new_goal_checker_id) {
|
||||
goal_.goal_checker_id = new_goal_checker_id;
|
||||
goal_updated_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
BT::NodeBuilder builder =
|
||||
[](const std::string & name, const BT::NodeConfiguration & config)
|
||||
{
|
||||
return std::make_unique<nav2_behavior_tree::FollowPathAction>(
|
||||
name, "follow_path", config);
|
||||
};
|
||||
|
||||
factory.registerBuilder<nav2_behavior_tree::FollowPathAction>(
|
||||
"FollowPath", builder);
|
||||
}
|
||||
@@ -0,0 +1,79 @@
|
||||
// Copyright (c) 2024 Marc Morcos
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <limits>
|
||||
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
GetPoseFromPath::GetPoseFromPath(
|
||||
const std::string & name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BT::ActionNodeBase(name, conf)
|
||||
{
|
||||
}
|
||||
|
||||
inline BT::NodeStatus GetPoseFromPath::tick()
|
||||
{
|
||||
setStatus(BT::NodeStatus::RUNNING);
|
||||
|
||||
nav_msgs::msg::Path input_path;
|
||||
getInput("path", input_path);
|
||||
|
||||
int pose_index;
|
||||
getInput("index", pose_index);
|
||||
|
||||
if (input_path.poses.empty()) {
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
// Account for negative indices
|
||||
if(pose_index < 0) {
|
||||
pose_index = input_path.poses.size() + pose_index;
|
||||
}
|
||||
|
||||
// out of bounds index
|
||||
if(pose_index < 0 || static_cast<unsigned>(pose_index) >= input_path.poses.size()) {
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
// extract pose
|
||||
geometry_msgs::msg::PoseStamped output_pose;
|
||||
output_pose = input_path.poses[pose_index];
|
||||
|
||||
// populate pose frame from path if necessary
|
||||
if(output_pose.header.frame_id.empty()) {
|
||||
output_pose.header.frame_id = input_path.header.frame_id;
|
||||
}
|
||||
|
||||
|
||||
setOutput("pose", output_pose);
|
||||
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
factory.registerNodeType<nav2_behavior_tree::GetPoseFromPath>("GetPoseFromPath");
|
||||
}
|
||||
@@ -0,0 +1,82 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
// Copyright (c) 2020 Pablo Iñigo Blasco
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
using std::placeholders::_1;
|
||||
|
||||
GoalCheckerSelector::GoalCheckerSelector(
|
||||
const std::string & name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BT::SyncActionNode(name, conf)
|
||||
{
|
||||
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
|
||||
|
||||
getInput("topic_name", topic_name_);
|
||||
|
||||
rclcpp::QoS qos(rclcpp::KeepLast(1));
|
||||
qos.transient_local().reliable();
|
||||
|
||||
goal_checker_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
|
||||
topic_name_, qos, std::bind(&GoalCheckerSelector::callbackGoalCheckerSelect, this, _1));
|
||||
}
|
||||
|
||||
BT::NodeStatus GoalCheckerSelector::tick()
|
||||
{
|
||||
rclcpp::spin_some(node_);
|
||||
|
||||
// This behavior always use the last selected goal checker received from the topic input.
|
||||
// When no input is specified it uses the default goal checker.
|
||||
// If the default goal checker is not specified then we work in "required goal checker mode":
|
||||
// In this mode, the behavior returns failure if the goal checker selection is not received from
|
||||
// the topic input.
|
||||
if (last_selected_goal_checker_.empty()) {
|
||||
std::string default_goal_checker;
|
||||
getInput("default_goal_checker", default_goal_checker);
|
||||
if (default_goal_checker.empty()) {
|
||||
return BT::NodeStatus::FAILURE;
|
||||
} else {
|
||||
last_selected_goal_checker_ = default_goal_checker;
|
||||
}
|
||||
}
|
||||
|
||||
setOutput("selected_goal_checker", last_selected_goal_checker_);
|
||||
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
void
|
||||
GoalCheckerSelector::callbackGoalCheckerSelect(const std_msgs::msg::String::SharedPtr msg)
|
||||
{
|
||||
last_selected_goal_checker_ = msg->data;
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
factory.registerNodeType<nav2_behavior_tree::GoalCheckerSelector>("GoalCheckerSelector");
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
// Copyright (c) 2021 Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
NavigateThroughPosesAction::NavigateThroughPosesAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtActionNode<nav2_msgs::action::NavigateThroughPoses>(xml_tag_name, action_name, conf)
|
||||
{
|
||||
}
|
||||
|
||||
void NavigateThroughPosesAction::on_tick()
|
||||
{
|
||||
if (!getInput("goals", goal_.poses)) {
|
||||
RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"NavigateThroughPosesAction: goal not provided");
|
||||
return;
|
||||
}
|
||||
getInput("behavior_tree", goal_.behavior_tree);
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
BT::NodeBuilder builder =
|
||||
[](const std::string & name, const BT::NodeConfiguration & config)
|
||||
{
|
||||
return std::make_unique<nav2_behavior_tree::NavigateThroughPosesAction>(
|
||||
name, "navigate_through_poses", config);
|
||||
};
|
||||
|
||||
factory.registerBuilder<nav2_behavior_tree::NavigateThroughPosesAction>(
|
||||
"NavigateThroughPoses", builder);
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
NavigateToPoseAction::NavigateToPoseAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtActionNode<nav2_msgs::action::NavigateToPose>(xml_tag_name, action_name, conf)
|
||||
{
|
||||
}
|
||||
|
||||
void NavigateToPoseAction::on_tick()
|
||||
{
|
||||
if (!getInput("goal", goal_.pose)) {
|
||||
RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"NavigateToPoseAction: goal not provided");
|
||||
return;
|
||||
}
|
||||
getInput("behavior_tree", goal_.behavior_tree);
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
BT::NodeBuilder builder =
|
||||
[](const std::string & name, const BT::NodeConfiguration & config)
|
||||
{
|
||||
return std::make_unique<nav2_behavior_tree::NavigateToPoseAction>(
|
||||
name, "navigate_to_pose", config);
|
||||
};
|
||||
|
||||
factory.registerBuilder<nav2_behavior_tree::NavigateToPoseAction>(
|
||||
"NavigateToPose", builder);
|
||||
}
|
||||
@@ -0,0 +1,91 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
// Copyright (c) 2020 Pablo Iñigo Blasco
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/planner_selector_node.hpp"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
using std::placeholders::_1;
|
||||
|
||||
PlannerSelector::PlannerSelector(
|
||||
const std::string & name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BT::SyncActionNode(name, conf)
|
||||
{
|
||||
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
|
||||
callback_group_ = node_->create_callback_group(
|
||||
rclcpp::CallbackGroupType::MutuallyExclusive,
|
||||
false);
|
||||
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
|
||||
|
||||
getInput("topic_name", topic_name_);
|
||||
|
||||
rclcpp::QoS qos(rclcpp::KeepLast(1));
|
||||
qos.transient_local().reliable();
|
||||
|
||||
rclcpp::SubscriptionOptions sub_option;
|
||||
sub_option.callback_group = callback_group_;
|
||||
planner_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
|
||||
topic_name_,
|
||||
qos,
|
||||
std::bind(&PlannerSelector::callbackPlannerSelect, this, _1),
|
||||
sub_option);
|
||||
}
|
||||
|
||||
BT::NodeStatus PlannerSelector::tick()
|
||||
{
|
||||
callback_group_executor_.spin_some();
|
||||
|
||||
// This behavior always use the last selected planner received from the topic input.
|
||||
// When no input is specified it uses the default planner.
|
||||
// If the default planner is not specified then we work in "required planner mode":
|
||||
// In this mode, the behavior returns failure if the planner selection is not received from
|
||||
// the topic input.
|
||||
if (last_selected_planner_.empty()) {
|
||||
std::string default_planner;
|
||||
getInput("default_planner", default_planner);
|
||||
if (default_planner.empty()) {
|
||||
return BT::NodeStatus::FAILURE;
|
||||
} else {
|
||||
last_selected_planner_ = default_planner;
|
||||
}
|
||||
}
|
||||
|
||||
setOutput("selected_planner", last_selected_planner_);
|
||||
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
void
|
||||
PlannerSelector::callbackPlannerSelect(const std_msgs::msg::String::SharedPtr msg)
|
||||
{
|
||||
last_selected_planner_ = msg->data;
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
factory.registerNodeType<nav2_behavior_tree::PlannerSelector>("PlannerSelector");
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
// Copyright (c) 2024 Open Navigation LLC
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
using std::placeholders::_1;
|
||||
|
||||
ProgressCheckerSelector::ProgressCheckerSelector(
|
||||
const std::string & name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BT::SyncActionNode(name, conf)
|
||||
{
|
||||
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
|
||||
|
||||
getInput("topic_name", topic_name_);
|
||||
|
||||
rclcpp::QoS qos(rclcpp::KeepLast(1));
|
||||
qos.transient_local().reliable();
|
||||
|
||||
progress_checker_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
|
||||
topic_name_, qos, std::bind(&ProgressCheckerSelector::callbackProgressCheckerSelect, this, _1));
|
||||
}
|
||||
|
||||
BT::NodeStatus ProgressCheckerSelector::tick()
|
||||
{
|
||||
rclcpp::spin_some(node_);
|
||||
|
||||
// This behavior always use the last selected progress checker received from the topic input.
|
||||
// When no input is specified it uses the default goaprogressl checker.
|
||||
// If the default progress checker is not specified then we work in
|
||||
// "required progress checker mode": In this mode, the behavior returns failure if the progress
|
||||
// checker selection is not received from the topic input.
|
||||
if (last_selected_progress_checker_.empty()) {
|
||||
std::string default_progress_checker;
|
||||
getInput("default_progress_checker", default_progress_checker);
|
||||
if (default_progress_checker.empty()) {
|
||||
return BT::NodeStatus::FAILURE;
|
||||
} else {
|
||||
last_selected_progress_checker_ = default_progress_checker;
|
||||
}
|
||||
}
|
||||
|
||||
setOutput("selected_progress_checker", last_selected_progress_checker_);
|
||||
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
void
|
||||
ProgressCheckerSelector::callbackProgressCheckerSelect(const std_msgs::msg::String::SharedPtr msg)
|
||||
{
|
||||
last_selected_progress_checker_ = msg->data;
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
factory.registerNodeType<nav2_behavior_tree::ProgressCheckerSelector>("ProgressCheckerSelector");
|
||||
}
|
||||
+34
@@ -0,0 +1,34 @@
|
||||
// Copyright (c) 2019 Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include "nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
ReinitializeGlobalLocalizationService::ReinitializeGlobalLocalizationService(
|
||||
const std::string & service_node_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtServiceNode<std_srvs::srv::Empty>(service_node_name, conf)
|
||||
{}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
factory.registerNodeType<nav2_behavior_tree::ReinitializeGlobalLocalizationService>(
|
||||
"ReinitializeGlobalLocalization");
|
||||
}
|
||||
@@ -0,0 +1,86 @@
|
||||
// Copyright (c) 2021 Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <limits>
|
||||
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
RemovePassedGoals::RemovePassedGoals(
|
||||
const std::string & name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BT::ActionNodeBase(name, conf),
|
||||
viapoint_achieved_radius_(0.5)
|
||||
{
|
||||
getInput("radius", viapoint_achieved_radius_);
|
||||
|
||||
getInput("global_frame", global_frame_);
|
||||
getInput("robot_base_frame", robot_base_frame_);
|
||||
tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
|
||||
auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
|
||||
node->get_parameter("transform_tolerance", transform_tolerance_);
|
||||
}
|
||||
|
||||
inline BT::NodeStatus RemovePassedGoals::tick()
|
||||
{
|
||||
setStatus(BT::NodeStatus::RUNNING);
|
||||
|
||||
Goals goal_poses;
|
||||
getInput("input_goals", goal_poses);
|
||||
|
||||
if (goal_poses.empty()) {
|
||||
setOutput("output_goals", goal_poses);
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
using namespace nav2_util::geometry_utils; // NOLINT
|
||||
|
||||
geometry_msgs::msg::PoseStamped current_pose;
|
||||
if (!nav2_util::getCurrentPose(
|
||||
current_pose, *tf_, global_frame_, robot_base_frame_,
|
||||
transform_tolerance_))
|
||||
{
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
double dist_to_goal;
|
||||
while (goal_poses.size() > 1) {
|
||||
dist_to_goal = euclidean_distance(goal_poses[0].pose, current_pose.pose);
|
||||
|
||||
if (dist_to_goal > viapoint_achieved_radius_) {
|
||||
break;
|
||||
}
|
||||
|
||||
goal_poses.erase(goal_poses.begin());
|
||||
}
|
||||
|
||||
setOutput("output_goals", goal_poses);
|
||||
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
factory.registerNodeType<nav2_behavior_tree::RemovePassedGoals>("RemovePassedGoals");
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
// Copyright (c) 2021 RoboTech Vision
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/smooth_path_action.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
SmoothPathAction::SmoothPathAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtActionNode<nav2_msgs::action::SmoothPath>(xml_tag_name, action_name, conf)
|
||||
{
|
||||
}
|
||||
|
||||
void SmoothPathAction::on_tick()
|
||||
{
|
||||
getInput("unsmoothed_path", goal_.path);
|
||||
getInput("smoother_id", goal_.smoother_id);
|
||||
double max_smoothing_duration;
|
||||
getInput("max_smoothing_duration", max_smoothing_duration);
|
||||
goal_.max_smoothing_duration = rclcpp::Duration::from_seconds(max_smoothing_duration);
|
||||
getInput("check_for_collisions", goal_.check_for_collisions);
|
||||
}
|
||||
|
||||
BT::NodeStatus SmoothPathAction::on_success()
|
||||
{
|
||||
setOutput("smoothed_path", result_.result->path);
|
||||
setOutput("smoothing_duration", rclcpp::Duration(result_.result->smoothing_duration).seconds());
|
||||
setOutput("was_completed", result_.result->was_completed);
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
BT::NodeBuilder builder =
|
||||
[](const std::string & name, const BT::NodeConfiguration & config)
|
||||
{
|
||||
return std::make_unique<nav2_behavior_tree::SmoothPathAction>(
|
||||
name, "smooth_path", config);
|
||||
};
|
||||
|
||||
factory.registerBuilder<nav2_behavior_tree::SmoothPathAction>(
|
||||
"SmoothPath", builder);
|
||||
}
|
||||
@@ -0,0 +1,92 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
// Copyright (c) 2020 Pablo Iñigo Blasco
|
||||
// Copyright (c) 2022 Owen Hooper
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/smoother_selector_node.hpp"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
using std::placeholders::_1;
|
||||
|
||||
SmootherSelector::SmootherSelector(
|
||||
const std::string & name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BT::SyncActionNode(name, conf)
|
||||
{
|
||||
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
|
||||
callback_group_ = node_->create_callback_group(
|
||||
rclcpp::CallbackGroupType::MutuallyExclusive,
|
||||
false);
|
||||
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
|
||||
|
||||
getInput("topic_name", topic_name_);
|
||||
|
||||
rclcpp::QoS qos(rclcpp::KeepLast(1));
|
||||
qos.transient_local().reliable();
|
||||
|
||||
rclcpp::SubscriptionOptions sub_option;
|
||||
sub_option.callback_group = callback_group_;
|
||||
smoother_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
|
||||
topic_name_,
|
||||
qos,
|
||||
std::bind(&SmootherSelector::callbackSmootherSelect, this, _1),
|
||||
sub_option);
|
||||
}
|
||||
|
||||
BT::NodeStatus SmootherSelector::tick()
|
||||
{
|
||||
callback_group_executor_.spin_some();
|
||||
|
||||
// This behavior always use the last selected smoother received from the topic input.
|
||||
// When no input is specified it uses the default smoother.
|
||||
// If the default smoother is not specified then we work in "required smoother mode":
|
||||
// In this mode, the behavior returns failure if the smoother selection is not received from
|
||||
// the topic input.
|
||||
if (last_selected_smoother_.empty()) {
|
||||
std::string default_smoother;
|
||||
getInput("default_smoother", default_smoother);
|
||||
if (default_smoother.empty()) {
|
||||
return BT::NodeStatus::FAILURE;
|
||||
} else {
|
||||
last_selected_smoother_ = default_smoother;
|
||||
}
|
||||
}
|
||||
|
||||
setOutput("selected_smoother", last_selected_smoother_);
|
||||
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
void
|
||||
SmootherSelector::callbackSmootherSelect(const std_msgs::msg::String::SharedPtr msg)
|
||||
{
|
||||
last_selected_smoother_ = msg->data;
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
factory.registerNodeType<nav2_behavior_tree::SmootherSelector>("SmootherSelector");
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/spin_action.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
SpinAction::SpinAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtActionNode<nav2_msgs::action::Spin>(xml_tag_name, action_name, conf)
|
||||
{
|
||||
double dist;
|
||||
getInput("spin_dist", dist);
|
||||
double time_allowance;
|
||||
getInput("time_allowance", time_allowance);
|
||||
goal_.target_yaw = dist;
|
||||
goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
|
||||
getInput("is_recovery", is_recovery_);
|
||||
}
|
||||
|
||||
void SpinAction::on_tick()
|
||||
{
|
||||
if (is_recovery_) {
|
||||
increment_recovery_count();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
BT::NodeBuilder builder =
|
||||
[](const std::string & name, const BT::NodeConfiguration & config)
|
||||
{
|
||||
return std::make_unique<nav2_behavior_tree::SpinAction>(name, "spin", config);
|
||||
};
|
||||
|
||||
factory.registerBuilder<nav2_behavior_tree::SpinAction>("Spin", builder);
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
// Copyright (c) 2022 Neobotix GmbH
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/spin_cancel_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
SpinCancel::SpinCancel(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtCancelActionNode<nav2_msgs::action::Spin>(xml_tag_name, action_name, conf)
|
||||
{
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
BT::NodeBuilder builder =
|
||||
[](const std::string & name, const BT::NodeConfiguration & config)
|
||||
{
|
||||
return std::make_unique<nav2_behavior_tree::SpinCancel>(
|
||||
name, "spin", config);
|
||||
};
|
||||
|
||||
factory.registerBuilder<nav2_behavior_tree::SpinCancel>(
|
||||
"CancelSpin", builder);
|
||||
}
|
||||
@@ -0,0 +1,89 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
// Copyright (c) 2020 Francisco Martin Rico
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <limits>
|
||||
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
#include "behaviortree_cpp_v3/decorator_node.h"
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
TruncatePath::TruncatePath(
|
||||
const std::string & name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BT::ActionNodeBase(name, conf),
|
||||
distance_(1.0)
|
||||
{
|
||||
getInput("distance", distance_);
|
||||
}
|
||||
|
||||
inline BT::NodeStatus TruncatePath::tick()
|
||||
{
|
||||
setStatus(BT::NodeStatus::RUNNING);
|
||||
|
||||
nav_msgs::msg::Path input_path;
|
||||
|
||||
getInput("input_path", input_path);
|
||||
|
||||
if (input_path.poses.empty()) {
|
||||
setOutput("output_path", input_path);
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PoseStamped final_pose = input_path.poses.back();
|
||||
|
||||
double distance_to_goal = nav2_util::geometry_utils::euclidean_distance(
|
||||
input_path.poses.back(), final_pose);
|
||||
|
||||
while (distance_to_goal < distance_ && input_path.poses.size() > 2) {
|
||||
input_path.poses.pop_back();
|
||||
distance_to_goal = nav2_util::geometry_utils::euclidean_distance(
|
||||
input_path.poses.back(), final_pose);
|
||||
}
|
||||
|
||||
double dx = final_pose.pose.position.x - input_path.poses.back().pose.position.x;
|
||||
double dy = final_pose.pose.position.y - input_path.poses.back().pose.position.y;
|
||||
|
||||
double final_angle = atan2(dy, dx);
|
||||
|
||||
if (std::isnan(final_angle) || std::isinf(final_angle)) {
|
||||
RCLCPP_WARN(
|
||||
config().blackboard->get<rclcpp::Node::SharedPtr>("node")->get_logger(),
|
||||
"Final angle is not valid while truncating path. Setting to 0.0");
|
||||
final_angle = 0.0;
|
||||
}
|
||||
|
||||
input_path.poses.back().pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
|
||||
final_angle);
|
||||
|
||||
setOutput("output_path", input_path);
|
||||
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
factory.registerNodeType<nav2_behavior_tree::TruncatePath>("TruncatePath");
|
||||
}
|
||||
@@ -0,0 +1,157 @@
|
||||
// Copyright (c) 2021 RoboTech Vision
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "behaviortree_cpp_v3/decorator_node.h"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
#include "tf2_ros/create_timer_ros.h"
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
TruncatePathLocal::TruncatePathLocal(
|
||||
const std::string & name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BT::ActionNodeBase(name, conf)
|
||||
{
|
||||
tf_buffer_ =
|
||||
config().blackboard->template get<std::shared_ptr<tf2_ros::Buffer>>(
|
||||
"tf_buffer");
|
||||
}
|
||||
|
||||
inline BT::NodeStatus TruncatePathLocal::tick()
|
||||
{
|
||||
setStatus(BT::NodeStatus::RUNNING);
|
||||
|
||||
double distance_forward, distance_backward;
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
double angular_distance_weight;
|
||||
double max_robot_pose_search_dist;
|
||||
|
||||
getInput("distance_forward", distance_forward);
|
||||
getInput("distance_backward", distance_backward);
|
||||
getInput("angular_distance_weight", angular_distance_weight);
|
||||
getInput("max_robot_pose_search_dist", max_robot_pose_search_dist);
|
||||
|
||||
bool path_pruning = std::isfinite(max_robot_pose_search_dist);
|
||||
nav_msgs::msg::Path new_path;
|
||||
getInput("input_path", new_path);
|
||||
if (!path_pruning || new_path != path_) {
|
||||
path_ = new_path;
|
||||
closest_pose_detection_begin_ = path_.poses.begin();
|
||||
}
|
||||
|
||||
if (!getRobotPose(path_.header.frame_id, pose)) {
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
if (path_.poses.empty()) {
|
||||
setOutput("output_path", path_);
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
auto closest_pose_detection_end = path_.poses.end();
|
||||
if (path_pruning) {
|
||||
closest_pose_detection_end = nav2_util::geometry_utils::first_after_integrated_distance(
|
||||
closest_pose_detection_begin_, path_.poses.end(), max_robot_pose_search_dist);
|
||||
}
|
||||
|
||||
// find the closest pose on the path
|
||||
auto current_pose = nav2_util::geometry_utils::min_by(
|
||||
closest_pose_detection_begin_, closest_pose_detection_end,
|
||||
[&pose, angular_distance_weight](const geometry_msgs::msg::PoseStamped & ps) {
|
||||
return poseDistance(pose, ps, angular_distance_weight);
|
||||
});
|
||||
|
||||
if (path_pruning) {
|
||||
closest_pose_detection_begin_ = current_pose;
|
||||
}
|
||||
|
||||
// expand forwards to extract desired length
|
||||
auto forward_pose_it = nav2_util::geometry_utils::first_after_integrated_distance(
|
||||
current_pose, path_.poses.end(), distance_forward);
|
||||
|
||||
// expand backwards to extract desired length
|
||||
// Note: current_pose + 1 is used because reverse iterator points to a cell before it
|
||||
auto backward_pose_it = nav2_util::geometry_utils::first_after_integrated_distance(
|
||||
std::reverse_iterator(current_pose + 1), path_.poses.rend(), distance_backward);
|
||||
|
||||
nav_msgs::msg::Path output_path;
|
||||
output_path.header = path_.header;
|
||||
output_path.poses = std::vector<geometry_msgs::msg::PoseStamped>(
|
||||
backward_pose_it.base(), forward_pose_it);
|
||||
setOutput("output_path", output_path);
|
||||
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
inline bool TruncatePathLocal::getRobotPose(
|
||||
std::string path_frame_id, geometry_msgs::msg::PoseStamped & pose)
|
||||
{
|
||||
if (!getInput("pose", pose)) {
|
||||
std::string robot_frame;
|
||||
if (!getInput("robot_frame", robot_frame)) {
|
||||
RCLCPP_ERROR(
|
||||
config().blackboard->get<rclcpp::Node::SharedPtr>("node")->get_logger(),
|
||||
"Neither pose nor robot_frame specified for %s", name().c_str());
|
||||
return false;
|
||||
}
|
||||
double transform_tolerance;
|
||||
getInput("transform_tolerance", transform_tolerance);
|
||||
if (!nav2_util::getCurrentPose(
|
||||
pose, *tf_buffer_, path_frame_id, robot_frame, transform_tolerance))
|
||||
{
|
||||
RCLCPP_WARN(
|
||||
config().blackboard->get<rclcpp::Node::SharedPtr>("node")->get_logger(),
|
||||
"Failed to lookup current robot pose for %s", name().c_str());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
double
|
||||
TruncatePathLocal::poseDistance(
|
||||
const geometry_msgs::msg::PoseStamped & pose1,
|
||||
const geometry_msgs::msg::PoseStamped & pose2,
|
||||
const double angular_distance_weight)
|
||||
{
|
||||
double dx = pose1.pose.position.x - pose2.pose.position.x;
|
||||
double dy = pose1.pose.position.y - pose2.pose.position.y;
|
||||
// taking angular distance into account in addition to spatial distance
|
||||
// (to improve picking a correct pose near cusps and loops)
|
||||
tf2::Quaternion q1;
|
||||
tf2::convert(pose1.pose.orientation, q1);
|
||||
tf2::Quaternion q2;
|
||||
tf2::convert(pose2.pose.orientation, q2);
|
||||
double da = angular_distance_weight * std::abs(q1.angleShortestPath(q2));
|
||||
return std::sqrt(dx * dx + dy * dy + da * da);
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory) {
|
||||
factory.registerNodeType<nav2_behavior_tree::TruncatePathLocal>(
|
||||
"TruncatePathLocal");
|
||||
}
|
||||
@@ -0,0 +1,58 @@
|
||||
// Copyright (c) 2018 Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/wait_action.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
WaitAction::WaitAction(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtActionNode<nav2_msgs::action::Wait>(xml_tag_name, action_name, conf)
|
||||
{
|
||||
int duration;
|
||||
getInput("wait_duration", duration);
|
||||
if (duration <= 0) {
|
||||
RCLCPP_WARN(
|
||||
node_->get_logger(), "Wait duration is negative or zero "
|
||||
"(%i). Setting to positive.", duration);
|
||||
duration *= -1;
|
||||
}
|
||||
|
||||
goal_.time.sec = duration;
|
||||
}
|
||||
|
||||
void WaitAction::on_tick()
|
||||
{
|
||||
increment_recovery_count();
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
BT::NodeBuilder builder =
|
||||
[](const std::string & name, const BT::NodeConfiguration & config)
|
||||
{
|
||||
return std::make_unique<nav2_behavior_tree::WaitAction>(name, "wait", config);
|
||||
};
|
||||
|
||||
factory.registerBuilder<nav2_behavior_tree::WaitAction>("Wait", builder);
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
// Copyright (c) 2022 Neobotix GmbH
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/plugins/action/wait_cancel_node.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
WaitCancel::WaitCancel(
|
||||
const std::string & xml_tag_name,
|
||||
const std::string & action_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BtCancelActionNode<nav2_msgs::action::Wait>(xml_tag_name, action_name, conf)
|
||||
{
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
BT::NodeBuilder builder =
|
||||
[](const std::string & name, const BT::NodeConfiguration & config)
|
||||
{
|
||||
return std::make_unique<nav2_behavior_tree::WaitCancel>(
|
||||
name, "wait", config);
|
||||
};
|
||||
|
||||
factory.registerBuilder<nav2_behavior_tree::WaitCancel>(
|
||||
"CancelWait", builder);
|
||||
}
|
||||
@@ -0,0 +1,86 @@
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
DistanceTraveledCondition::DistanceTraveledCondition(
|
||||
const std::string & condition_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BT::ConditionNode(condition_name, conf),
|
||||
distance_(1.0),
|
||||
transform_tolerance_(0.1),
|
||||
global_frame_("map"),
|
||||
robot_base_frame_("base_link")
|
||||
{
|
||||
getInput("distance", distance_);
|
||||
getInput("global_frame", global_frame_);
|
||||
getInput("robot_base_frame", robot_base_frame_);
|
||||
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
|
||||
tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
|
||||
node_->get_parameter("transform_tolerance", transform_tolerance_);
|
||||
}
|
||||
|
||||
BT::NodeStatus DistanceTraveledCondition::tick()
|
||||
{
|
||||
if (status() == BT::NodeStatus::IDLE) {
|
||||
if (!nav2_util::getCurrentPose(
|
||||
start_pose_, *tf_, global_frame_, robot_base_frame_,
|
||||
transform_tolerance_))
|
||||
{
|
||||
RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
|
||||
}
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
// Determine distance travelled since we've started this iteration
|
||||
geometry_msgs::msg::PoseStamped current_pose;
|
||||
if (!nav2_util::getCurrentPose(
|
||||
current_pose, *tf_, global_frame_, robot_base_frame_,
|
||||
transform_tolerance_))
|
||||
{
|
||||
RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
// Get euclidean distance
|
||||
auto travelled = nav2_util::geometry_utils::euclidean_distance(
|
||||
start_pose_.pose, current_pose.pose);
|
||||
|
||||
if (travelled < distance_) {
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
// Update start pose
|
||||
start_pose_ = current_pose;
|
||||
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
factory.registerNodeType<nav2_behavior_tree::DistanceTraveledCondition>("DistanceTraveled");
|
||||
}
|
||||
@@ -0,0 +1,61 @@
|
||||
// Copyright (c) 2021 Joshua Wallace
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
#include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
GloballyUpdatedGoalCondition::GloballyUpdatedGoalCondition(
|
||||
const std::string & condition_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BT::ConditionNode(condition_name, conf),
|
||||
first_time(true)
|
||||
{
|
||||
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
|
||||
}
|
||||
|
||||
BT::NodeStatus GloballyUpdatedGoalCondition::tick()
|
||||
{
|
||||
if (first_time) {
|
||||
first_time = false;
|
||||
config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", goals_);
|
||||
config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", goal_);
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
std::vector<geometry_msgs::msg::PoseStamped> current_goals;
|
||||
config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", current_goals);
|
||||
geometry_msgs::msg::PoseStamped current_goal;
|
||||
config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", current_goal);
|
||||
|
||||
if (goal_ != current_goal || goals_ != current_goals) {
|
||||
goal_ = current_goal;
|
||||
goals_ = current_goals;
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
factory.registerNodeType<nav2_behavior_tree::GloballyUpdatedGoalCondition>("GlobalUpdatedGoal");
|
||||
}
|
||||
@@ -0,0 +1,96 @@
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
#include "nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
GoalReachedCondition::GoalReachedCondition(
|
||||
const std::string & condition_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BT::ConditionNode(condition_name, conf),
|
||||
initialized_(false),
|
||||
global_frame_("map"),
|
||||
robot_base_frame_("base_link")
|
||||
{
|
||||
getInput("global_frame", global_frame_);
|
||||
getInput("robot_base_frame", robot_base_frame_);
|
||||
}
|
||||
|
||||
GoalReachedCondition::~GoalReachedCondition()
|
||||
{
|
||||
cleanup();
|
||||
}
|
||||
|
||||
BT::NodeStatus GoalReachedCondition::tick()
|
||||
{
|
||||
if (!initialized_) {
|
||||
initialize();
|
||||
}
|
||||
|
||||
if (isGoalReached()) {
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
void GoalReachedCondition::initialize()
|
||||
{
|
||||
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node_, "goal_reached_tol",
|
||||
rclcpp::ParameterValue(0.25));
|
||||
node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
|
||||
tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
|
||||
|
||||
node_->get_parameter("transform_tolerance", transform_tolerance_);
|
||||
|
||||
initialized_ = true;
|
||||
}
|
||||
|
||||
bool GoalReachedCondition::isGoalReached()
|
||||
{
|
||||
geometry_msgs::msg::PoseStamped current_pose;
|
||||
|
||||
if (!nav2_util::getCurrentPose(
|
||||
current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_))
|
||||
{
|
||||
RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
|
||||
return false;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PoseStamped goal;
|
||||
getInput("goal", goal);
|
||||
double dx = goal.pose.position.x - current_pose.pose.position.x;
|
||||
double dy = goal.pose.position.y - current_pose.pose.position.y;
|
||||
|
||||
return (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_);
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
factory.registerNodeType<nav2_behavior_tree::GoalReachedCondition>("GoalReached");
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
// Copyright (c) 2020 Aitor Miguel Blanco
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
GoalUpdatedCondition::GoalUpdatedCondition(
|
||||
const std::string & condition_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BT::ConditionNode(condition_name, conf)
|
||||
{}
|
||||
|
||||
BT::NodeStatus GoalUpdatedCondition::tick()
|
||||
{
|
||||
if (status() == BT::NodeStatus::IDLE) {
|
||||
config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", goals_);
|
||||
config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", goal_);
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
std::vector<geometry_msgs::msg::PoseStamped> current_goals;
|
||||
config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", current_goals);
|
||||
geometry_msgs::msg::PoseStamped current_goal;
|
||||
config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", current_goal);
|
||||
|
||||
if (goal_ != current_goal || goals_ != current_goals) {
|
||||
goal_ = current_goal;
|
||||
goals_ = current_goals;
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
factory.registerNodeType<nav2_behavior_tree::GoalUpdatedCondition>("GoalUpdated");
|
||||
}
|
||||
@@ -0,0 +1,34 @@
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node)
|
||||
{
|
||||
auto initPoseReceived = tree_node.config().blackboard->get<bool>("initial_pose_received");
|
||||
return initPoseReceived ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
factory.registerSimpleCondition(
|
||||
"InitialPoseReceived",
|
||||
std::bind(&nav2_behavior_tree::initialPoseReceived, std::placeholders::_1));
|
||||
}
|
||||
@@ -0,0 +1,66 @@
|
||||
// Copyright (c) 2023 Alberto J. Tudela Roldán
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp"
|
||||
|
||||
namespace nav2_behavior_tree
|
||||
{
|
||||
|
||||
IsBatteryChargingCondition::IsBatteryChargingCondition(
|
||||
const std::string & condition_name,
|
||||
const BT::NodeConfiguration & conf)
|
||||
: BT::ConditionNode(condition_name, conf),
|
||||
battery_topic_("/battery_status"),
|
||||
is_battery_charging_(false)
|
||||
{
|
||||
getInput("battery_topic", battery_topic_);
|
||||
auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
|
||||
callback_group_ = node->create_callback_group(
|
||||
rclcpp::CallbackGroupType::MutuallyExclusive,
|
||||
false);
|
||||
callback_group_executor_.add_callback_group(callback_group_, node->get_node_base_interface());
|
||||
|
||||
rclcpp::SubscriptionOptions sub_option;
|
||||
sub_option.callback_group = callback_group_;
|
||||
battery_sub_ = node->create_subscription<sensor_msgs::msg::BatteryState>(
|
||||
battery_topic_,
|
||||
rclcpp::SystemDefaultsQoS(),
|
||||
std::bind(&IsBatteryChargingCondition::batteryCallback, this, std::placeholders::_1),
|
||||
sub_option);
|
||||
}
|
||||
|
||||
BT::NodeStatus IsBatteryChargingCondition::tick()
|
||||
{
|
||||
callback_group_executor_.spin_some();
|
||||
if (is_battery_charging_) {
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
void IsBatteryChargingCondition::batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg)
|
||||
{
|
||||
is_battery_charging_ =
|
||||
(msg->power_supply_status == sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING);
|
||||
}
|
||||
|
||||
} // namespace nav2_behavior_tree
|
||||
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
factory.registerNodeType<nav2_behavior_tree::IsBatteryChargingCondition>("IsBatteryCharging");
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user