add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,94 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(nav2_bt_navigator)
|
||||
|
||||
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(rclcpp_components REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(nav2_behavior_tree REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(nav2_msgs REQUIRED)
|
||||
find_package(behaviortree_cpp_v3 REQUIRED)
|
||||
find_package(std_srvs REQUIRED)
|
||||
find_package(nav2_util REQUIRED)
|
||||
find_package(nav2_core REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
|
||||
nav2_package()
|
||||
|
||||
include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
set(executable_name bt_navigator)
|
||||
|
||||
add_executable(${executable_name}
|
||||
src/main.cpp
|
||||
)
|
||||
|
||||
set(library_name ${executable_name}_core)
|
||||
|
||||
set(dependencies
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
rclcpp_lifecycle
|
||||
rclcpp_components
|
||||
std_msgs
|
||||
geometry_msgs
|
||||
nav2_behavior_tree
|
||||
nav_msgs
|
||||
nav2_msgs
|
||||
behaviortree_cpp_v3
|
||||
std_srvs
|
||||
nav2_util
|
||||
nav2_core
|
||||
tf2_ros
|
||||
)
|
||||
|
||||
add_library(${library_name} SHARED
|
||||
src/bt_navigator.cpp
|
||||
src/navigators/navigate_to_pose.cpp
|
||||
src/navigators/navigate_through_poses.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(${executable_name}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
target_link_libraries(${executable_name} ${library_name})
|
||||
|
||||
ament_target_dependencies(${library_name}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
rclcpp_components_register_nodes(${library_name} "nav2_bt_navigator::BtNavigator")
|
||||
|
||||
install(TARGETS ${library_name}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
|
||||
install(TARGETS ${executable_name}
|
||||
RUNTIME DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include/
|
||||
)
|
||||
|
||||
install(DIRECTORY behavior_trees DESTINATION share/${PROJECT_NAME})
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_export_include_directories(include)
|
||||
ament_export_libraries(${library_name})
|
||||
ament_export_dependencies(${dependencies})
|
||||
ament_package()
|
||||
@@ -0,0 +1,9 @@
|
||||
# BT Navigator
|
||||
|
||||
The BT Navigator (Behavior Tree Navigator) module implements the NavigateToPose and NavigateThroughPoses task interfaces. It is a [Behavior Tree](https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/docs/BT_basics.md)-based implementation of navigation that is intended to allow for flexibility in the navigation task and provide a way to easily specify complex robot behaviors.
|
||||
|
||||
See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-bt-navigator.html) for additional parameter descriptions, as well as the [Nav2 Behavior Tree Explanation](https://docs.nav2.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package.
|
||||
|
||||
## Overview
|
||||
|
||||
The BT Navigator receives a goal pose and navigates the robot to the specified destination(s). To do so, the module reads an XML description of the Behavior Tree from a file, as specified by a Node parameter, and passes that to a generic [BehaviorTreeEngine class](../nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp) which uses the [Behavior-Tree.CPP library](https://github.com/BehaviorTree/BehaviorTree.CPP) to dynamically create and execute the BT. The BT XML can also be specified on a per-task basis so that your robot may have many different types of navigation or autonomy behaviors on a per-task basis.
|
||||
@@ -0,0 +1,21 @@
|
||||
<!--
|
||||
This Behavior Tree follows a dynamic pose to a certain distance
|
||||
-->
|
||||
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<RateController hz="1.0">
|
||||
<Sequence>
|
||||
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
|
||||
<ComputePathToPose goal="{updated_goal}" path="{path}" planner_id="GridBased"/>
|
||||
</GoalUpdater>
|
||||
<TruncatePath distance="1.0" input_path="{path}" output_path="{truncated_path}"/>
|
||||
</Sequence>
|
||||
</RateController>
|
||||
<KeepRunningUntilFailure>
|
||||
<FollowPath path="{truncated_path}" controller_id="FollowPath"/>
|
||||
</KeepRunningUntilFailure>
|
||||
</PipelineSequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
+46
@@ -0,0 +1,46 @@
|
||||
<!--
|
||||
This Behavior Tree replans the global path once every 15 seconds or if the path becomes invalid. It also has
|
||||
recovery actions specific to planning / control as well as general system issues.
|
||||
This will be continuous if a kinematically valid planner is selected.
|
||||
-->
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<RateController hz="2.0">
|
||||
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
|
||||
<Fallback>
|
||||
<ReactiveSequence>
|
||||
<Inverter>
|
||||
<PathExpiringTimer seconds="10" path="{path}"/>
|
||||
</Inverter>
|
||||
<Inverter>
|
||||
<GlobalUpdatedGoal/>
|
||||
</Inverter>
|
||||
<IsPathValid path="{path}"/>
|
||||
</ReactiveSequence>
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
</Fallback>
|
||||
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||
</RecoveryNode>
|
||||
</RateController>
|
||||
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
</RecoveryNode>
|
||||
</PipelineSequence>
|
||||
<ReactiveFallback name="RecoveryFallback">
|
||||
<GoalUpdated/>
|
||||
<RoundRobin name="RecoveryActions">
|
||||
<Sequence name="ClearingActions">
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||
</Sequence>
|
||||
<Spin spin_dist="1.57"/>
|
||||
<Wait wait_duration="5"/>
|
||||
<BackUp backup_dist="0.30" backup_speed="0.05"/>
|
||||
</RoundRobin>
|
||||
</ReactiveFallback>
|
||||
</RecoveryNode>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
+38
@@ -0,0 +1,38 @@
|
||||
|
||||
<!--
|
||||
This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously
|
||||
and it also has recovery actions specific to planning / control as well as general system issues.
|
||||
-->
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<RateController hz="0.333">
|
||||
<RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
|
||||
<ReactiveSequence>
|
||||
<RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
|
||||
<ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
|
||||
</ReactiveSequence>
|
||||
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||
</RecoveryNode>
|
||||
</RateController>
|
||||
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
</RecoveryNode>
|
||||
</PipelineSequence>
|
||||
<ReactiveFallback name="RecoveryFallback">
|
||||
<GoalUpdated/>
|
||||
<RoundRobin name="RecoveryActions">
|
||||
<Sequence name="ClearingActions">
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||
</Sequence>
|
||||
<Spin spin_dist="1.57"/>
|
||||
<Wait wait_duration="5"/>
|
||||
<BackUp backup_dist="0.30" backup_speed="0.05"/>
|
||||
</RoundRobin>
|
||||
</ReactiveFallback>
|
||||
</RecoveryNode>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
+36
@@ -0,0 +1,36 @@
|
||||
|
||||
<!--
|
||||
This Behavior Tree replans the global path periodically at 1 Hz and it also has
|
||||
recovery actions specific to planning / control as well as general system issues.
|
||||
This will be continuous if a kinematically valid planner is selected.
|
||||
-->
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<RateController hz="1.0">
|
||||
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||
</RecoveryNode>
|
||||
</RateController>
|
||||
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
</RecoveryNode>
|
||||
</PipelineSequence>
|
||||
<ReactiveFallback name="RecoveryFallback">
|
||||
<GoalUpdated/>
|
||||
<RoundRobin name="RecoveryActions">
|
||||
<Sequence name="ClearingActions">
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||
</Sequence>
|
||||
<Spin spin_dist="1.57"/>
|
||||
<Wait wait_duration="5"/>
|
||||
<BackUp backup_dist="0.30" backup_speed="0.05"/>
|
||||
</RoundRobin>
|
||||
</ReactiveFallback>
|
||||
</RecoveryNode>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
+47
@@ -0,0 +1,47 @@
|
||||
|
||||
<!--
|
||||
This BT has all the functionalities of navigate_to_pose_w_replanning_and_recovery.xml,
|
||||
with an additional feature to cancel the control closer to the goal proximity and
|
||||
make the robot wait for a specific time, to see if the obstacle clears out before
|
||||
navigating along a significantly longer path to reach the goal location.
|
||||
-->
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<RateController hz="1.0">
|
||||
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||
</RecoveryNode>
|
||||
</RateController>
|
||||
<ReactiveSequence name="MonitorAndFollowPath">
|
||||
<PathLongerOnApproach path="{path}" prox_len="3.0" length_factor="2.0">
|
||||
<RetryUntilSuccessful num_attempts="1">
|
||||
<SequenceStar name="CancelingControlAndWait">
|
||||
<CancelControl name="ControlCancel"/>
|
||||
<Wait wait_duration="5"/>
|
||||
</SequenceStar>
|
||||
</RetryUntilSuccessful>
|
||||
</PathLongerOnApproach>
|
||||
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
</RecoveryNode>
|
||||
</ReactiveSequence>
|
||||
</PipelineSequence>
|
||||
<ReactiveFallback name="RecoveryFallback">
|
||||
<GoalUpdated/>
|
||||
<RoundRobin name="RecoveryActions">
|
||||
<Sequence name="ClearingActions">
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||
</Sequence>
|
||||
<Spin spin_dist="1.57"/>
|
||||
<Wait wait_duration="5"/>
|
||||
<BackUp backup_dist="0.30" backup_speed="0.05"/>
|
||||
</RoundRobin>
|
||||
</ReactiveFallback>
|
||||
</RecoveryNode>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
+44
@@ -0,0 +1,44 @@
|
||||
|
||||
<!--
|
||||
This Behavior Tree replans the global path only if the path becomes invalid and it also has
|
||||
recovery actions specific to planning / control as well as general system issues.
|
||||
This will be continuous if a kinematically valid planner is selected.
|
||||
-->
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||
<PipelineSequence>
|
||||
<RateController hz="1.0" name="RateControllerComputePathToPose">
|
||||
<RecoveryNode number_of_retries="1" name="RecoveryComputePathToPose">
|
||||
<Fallback name="FallbackComputePathToPose">
|
||||
<ReactiveSequence name="CheckIfNewPathNeeded">
|
||||
<Inverter>
|
||||
<GlobalUpdatedGoal/>
|
||||
</Inverter>
|
||||
<IsPathValid path="{path}"/>
|
||||
</ReactiveSequence>
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
</Fallback>
|
||||
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||
</RecoveryNode>
|
||||
</RateController>
|
||||
<RecoveryNode number_of_retries="1" name="RecoveryFollowPath">
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
</RecoveryNode>
|
||||
</PipelineSequence>
|
||||
<ReactiveFallback name="FallbackRecoveries">
|
||||
<GoalUpdated/>
|
||||
<RoundRobin name="RecoveryActions">
|
||||
<Sequence name="ClearingActions">
|
||||
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||
</Sequence>
|
||||
<Spin name="SpinRecovery" spin_dist="1.57"/>
|
||||
<Wait name="WaitRecovery" wait_duration="5"/>
|
||||
<BackUp name="BackUpRecovery" backup_dist="0.30" backup_speed="0.05"/>
|
||||
</RoundRobin>
|
||||
</ReactiveFallback>
|
||||
</RecoveryNode>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -0,0 +1,14 @@
|
||||
<!--
|
||||
This Behavior Tree replans the global path after every 1m.
|
||||
-->
|
||||
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<DistanceController distance="1.0">
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
</DistanceController>
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
</PipelineSequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
+14
@@ -0,0 +1,14 @@
|
||||
<!--
|
||||
This Behavior Tree replans the global path only when the goal is updated.
|
||||
-->
|
||||
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<GoalUpdatedController>
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
</GoalUpdatedController>
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
</PipelineSequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
+21
@@ -0,0 +1,21 @@
|
||||
<!--
|
||||
This Behavior Tree replans the global path only if the path becomes invalid
|
||||
-->
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<RateController hz="1.0">
|
||||
<Fallback>
|
||||
<ReactiveSequence>
|
||||
<Inverter>
|
||||
<GlobalUpdatedGoal/>
|
||||
</Inverter>
|
||||
<IsPathValid path="{path}"/>
|
||||
</ReactiveSequence>
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
</Fallback>
|
||||
</RateController>
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
</PipelineSequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -0,0 +1,14 @@
|
||||
<!--
|
||||
This Behavior Tree replans the global path periodically proprortional to speed.
|
||||
-->
|
||||
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<SpeedController min_rate="0.1" max_rate="1.0" min_speed="0.0" max_speed="0.26">
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
</SpeedController>
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
</PipelineSequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -0,0 +1,14 @@
|
||||
<!--
|
||||
This Behavior Tree replans the global path periodically at 1 Hz.
|
||||
-->
|
||||
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<PipelineSequence name="NavigateWithReplanning">
|
||||
<RateController hz="1.0">
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
</RateController>
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
</PipelineSequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -0,0 +1,20 @@
|
||||
<!--
|
||||
his Behavior Tree drives in a square for odometry calibration experiments
|
||||
-->
|
||||
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<Repeat num_cycles="3">
|
||||
<Sequence name="Drive in a square">
|
||||
<DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
|
||||
<Spin spin_dist="1.570796" is_recovery="false"/>
|
||||
<DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
|
||||
<Spin spin_dist="1.570796" is_recovery="false"/>
|
||||
<DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
|
||||
<Spin spin_dist="1.570796" is_recovery="false"/>
|
||||
<DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
|
||||
<Spin spin_dist="1.570796" is_recovery="false"/>
|
||||
</Sequence>
|
||||
</Repeat>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 42 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 7.1 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 26 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 26 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 160 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 150 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 8.0 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 21 KiB |
@@ -0,0 +1,109 @@
|
||||
// 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_BT_NAVIGATOR__BT_NAVIGATOR_HPP_
|
||||
#define NAV2_BT_NAVIGATOR__BT_NAVIGATOR_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_util/odometry_utils.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
#include "tf2_ros/create_timer_ros.h"
|
||||
#include "nav2_bt_navigator/navigators/navigate_to_pose.hpp"
|
||||
#include "nav2_bt_navigator/navigators/navigate_through_poses.hpp"
|
||||
|
||||
namespace nav2_bt_navigator
|
||||
{
|
||||
|
||||
/**
|
||||
* @class nav2_bt_navigator::BtNavigator
|
||||
* @brief An action server that uses behavior tree for navigating a robot to its
|
||||
* goal position.
|
||||
*/
|
||||
class BtNavigator : public nav2_util::LifecycleNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A constructor for nav2_bt_navigator::BtNavigator class
|
||||
* @param options Additional options to control creation of the node.
|
||||
*/
|
||||
explicit BtNavigator(rclcpp::NodeOptions options = rclcpp::NodeOptions());
|
||||
/**
|
||||
* @brief A destructor for nav2_bt_navigator::BtNavigator class
|
||||
*/
|
||||
~BtNavigator();
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Configures member variables
|
||||
*
|
||||
* Initializes action server for "NavigationToPose"; subscription to
|
||||
* "goal_sub"; and builds behavior tree from xml file.
|
||||
* @param state Reference to LifeCycle node state
|
||||
* @return SUCCESS or FAILURE
|
||||
*/
|
||||
nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
|
||||
/**
|
||||
* @brief Activates action server
|
||||
* @param state Reference to LifeCycle node state
|
||||
* @return SUCCESS or FAILURE
|
||||
*/
|
||||
nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
|
||||
/**
|
||||
* @brief Deactivates action server
|
||||
* @param state Reference to LifeCycle node state
|
||||
* @return SUCCESS or FAILURE
|
||||
*/
|
||||
nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
|
||||
/**
|
||||
* @brief Resets member variables
|
||||
* @param state Reference to LifeCycle node state
|
||||
* @return SUCCESS or FAILURE
|
||||
*/
|
||||
nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
|
||||
/**
|
||||
* @brief Called when in shutdown state
|
||||
* @param state Reference to LifeCycle node state
|
||||
* @return SUCCESS or FAILURE
|
||||
*/
|
||||
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
|
||||
|
||||
// To handle all the BT related execution
|
||||
std::unique_ptr<nav2_bt_navigator::Navigator<nav2_msgs::action::NavigateToPose>> pose_navigator_;
|
||||
std::unique_ptr<nav2_bt_navigator::Navigator<nav2_msgs::action::NavigateThroughPoses>>
|
||||
poses_navigator_;
|
||||
nav2_bt_navigator::NavigatorMuxer plugin_muxer_;
|
||||
|
||||
// Odometry smoother object
|
||||
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
|
||||
|
||||
// Metrics for feedback
|
||||
std::string robot_frame_;
|
||||
std::string global_frame_;
|
||||
double transform_tolerance_;
|
||||
std::string odom_topic_;
|
||||
|
||||
// Spinning transform that can be used by the BT nodes
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||
};
|
||||
|
||||
} // namespace nav2_bt_navigator
|
||||
|
||||
#endif // NAV2_BT_NAVIGATOR__BT_NAVIGATOR_HPP_
|
||||
@@ -0,0 +1,339 @@
|
||||
// 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_BT_NAVIGATOR__NAVIGATOR_HPP_
|
||||
#define NAV2_BT_NAVIGATOR__NAVIGATOR_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <mutex>
|
||||
|
||||
#include "nav2_util/odometry_utils.hpp"
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
#include "pluginlib/class_loader.hpp"
|
||||
#include "nav2_behavior_tree/bt_action_server.hpp"
|
||||
|
||||
namespace nav2_bt_navigator
|
||||
{
|
||||
|
||||
/**
|
||||
* @struct FeedbackUtils
|
||||
* @brief Navigator feedback utilities required to get transforms and reference frames.
|
||||
*/
|
||||
struct FeedbackUtils
|
||||
{
|
||||
std::string robot_frame;
|
||||
std::string global_frame;
|
||||
double transform_tolerance;
|
||||
std::shared_ptr<tf2_ros::Buffer> tf;
|
||||
};
|
||||
|
||||
/**
|
||||
* @class NavigatorMuxer
|
||||
* @brief A class to control the state of the BT navigator by allowing only a single
|
||||
* plugin to be processed at a time.
|
||||
*/
|
||||
class NavigatorMuxer
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief A Navigator Muxer constructor
|
||||
*/
|
||||
NavigatorMuxer()
|
||||
: current_navigator_(std::string("")) {}
|
||||
|
||||
/**
|
||||
* @brief Get the navigator muxer state
|
||||
* @return bool If a navigator is in progress
|
||||
*/
|
||||
bool isNavigating()
|
||||
{
|
||||
std::scoped_lock l(mutex_);
|
||||
return !current_navigator_.empty();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Start navigating with a given navigator
|
||||
* @param string Name of the navigator to start
|
||||
*/
|
||||
void startNavigating(const std::string & navigator_name)
|
||||
{
|
||||
std::scoped_lock l(mutex_);
|
||||
if (!current_navigator_.empty()) {
|
||||
RCLCPP_ERROR(
|
||||
rclcpp::get_logger("NavigatorMutex"),
|
||||
"Major error! Navigation requested while another navigation"
|
||||
" task is in progress! This likely occurred from an incorrect"
|
||||
"implementation of a navigator plugin.");
|
||||
}
|
||||
current_navigator_ = navigator_name;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Stop navigating with a given navigator
|
||||
* @param string Name of the navigator ending task
|
||||
*/
|
||||
void stopNavigating(const std::string & navigator_name)
|
||||
{
|
||||
std::scoped_lock l(mutex_);
|
||||
if (current_navigator_ != navigator_name) {
|
||||
RCLCPP_ERROR(
|
||||
rclcpp::get_logger("NavigatorMutex"),
|
||||
"Major error! Navigation stopped while another navigation"
|
||||
" task is in progress! This likely occurred from an incorrect"
|
||||
"implementation of a navigator plugin.");
|
||||
} else {
|
||||
current_navigator_ = std::string("");
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
std::string current_navigator_;
|
||||
std::mutex mutex_;
|
||||
};
|
||||
|
||||
/**
|
||||
* @class Navigator
|
||||
* @brief Navigator interface that acts as a base class for all BT-based Navigator action's plugins
|
||||
*/
|
||||
template<class ActionT>
|
||||
class Navigator
|
||||
{
|
||||
public:
|
||||
using Ptr = std::shared_ptr<nav2_bt_navigator::Navigator<ActionT>>;
|
||||
|
||||
/**
|
||||
* @brief A Navigator constructor
|
||||
*/
|
||||
Navigator()
|
||||
{
|
||||
plugin_muxer_ = nullptr;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Virtual destructor
|
||||
*/
|
||||
virtual ~Navigator() = default;
|
||||
|
||||
/**
|
||||
* @brief Configuration to setup the navigator's backend BT and actions
|
||||
* @param parent_node The ROS parent node to utilize
|
||||
* @param plugin_lib_names a vector of plugin shared libraries to load
|
||||
* @param feedback_utils Some utilities useful for navigators to have
|
||||
* @param plugin_muxer The muxing object to ensure only one navigator
|
||||
* can be active at a time
|
||||
* @param odom_smoother Object to get current smoothed robot's speed
|
||||
* @return bool If successful
|
||||
*/
|
||||
bool on_configure(
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node,
|
||||
const std::vector<std::string> & plugin_lib_names,
|
||||
const FeedbackUtils & feedback_utils,
|
||||
nav2_bt_navigator::NavigatorMuxer * plugin_muxer,
|
||||
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother)
|
||||
{
|
||||
auto node = parent_node.lock();
|
||||
logger_ = node->get_logger();
|
||||
clock_ = node->get_clock();
|
||||
feedback_utils_ = feedback_utils;
|
||||
plugin_muxer_ = plugin_muxer;
|
||||
|
||||
// get the default behavior tree for this navigator
|
||||
std::string default_bt_xml_filename = getDefaultBTFilepath(parent_node);
|
||||
|
||||
// Create the Behavior Tree Action Server for this navigator
|
||||
bt_action_server_ = std::make_unique<nav2_behavior_tree::BtActionServer<ActionT>>(
|
||||
node,
|
||||
getName(),
|
||||
plugin_lib_names,
|
||||
default_bt_xml_filename,
|
||||
std::bind(&Navigator::onGoalReceived, this, std::placeholders::_1),
|
||||
std::bind(&Navigator::onLoop, this),
|
||||
std::bind(&Navigator::onPreempt, this, std::placeholders::_1),
|
||||
std::bind(&Navigator::onCompletion, this, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
bool ok = true;
|
||||
if (!bt_action_server_->on_configure()) {
|
||||
ok = false;
|
||||
}
|
||||
|
||||
BT::Blackboard::Ptr blackboard = bt_action_server_->getBlackboard();
|
||||
blackboard->set<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer", feedback_utils.tf); // NOLINT
|
||||
blackboard->set<bool>("initial_pose_received", false); // NOLINT
|
||||
blackboard->set<int>("number_recoveries", 0); // NOLINT
|
||||
blackboard->set<std::shared_ptr<nav2_util::OdomSmoother>>("odom_smoother", odom_smoother); // NOLINT
|
||||
|
||||
return configure(parent_node, odom_smoother) && ok;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Activation of the navigator's backend BT and actions
|
||||
* @return bool If successful
|
||||
*/
|
||||
bool on_activate()
|
||||
{
|
||||
bool ok = true;
|
||||
|
||||
if (!bt_action_server_->on_activate()) {
|
||||
ok = false;
|
||||
}
|
||||
|
||||
return activate() && ok;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Deactivation of the navigator's backend BT and actions
|
||||
* @return bool If successful
|
||||
*/
|
||||
bool on_deactivate()
|
||||
{
|
||||
bool ok = true;
|
||||
if (!bt_action_server_->on_deactivate()) {
|
||||
ok = false;
|
||||
}
|
||||
|
||||
return deactivate() && ok;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cleanup a navigator
|
||||
* @return bool If successful
|
||||
*/
|
||||
bool on_cleanup()
|
||||
{
|
||||
bool ok = true;
|
||||
if (!bt_action_server_->on_cleanup()) {
|
||||
ok = false;
|
||||
}
|
||||
|
||||
bt_action_server_.reset();
|
||||
|
||||
return cleanup() && ok;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the action name of this navigator to expose
|
||||
* @return string Name of action to expose
|
||||
*/
|
||||
virtual std::string getName() = 0;
|
||||
|
||||
virtual std::string getDefaultBTFilepath(rclcpp_lifecycle::LifecycleNode::WeakPtr node) = 0;
|
||||
|
||||
/**
|
||||
* @brief Get the action server
|
||||
* @return Action server pointer
|
||||
*/
|
||||
std::unique_ptr<nav2_behavior_tree::BtActionServer<ActionT>> & getActionServer()
|
||||
{
|
||||
return bt_action_server_;
|
||||
}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief An intermediate goal reception function to mux navigators.
|
||||
*/
|
||||
bool onGoalReceived(typename ActionT::Goal::ConstSharedPtr goal)
|
||||
{
|
||||
if (plugin_muxer_->isNavigating()) {
|
||||
RCLCPP_ERROR(
|
||||
logger_,
|
||||
"Requested navigation from %s while another navigator is processing,"
|
||||
" rejecting request.", getName().c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
bool goal_accepted = goalReceived(goal);
|
||||
|
||||
if (goal_accepted) {
|
||||
plugin_muxer_->startNavigating(getName());
|
||||
}
|
||||
|
||||
return goal_accepted;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief An intermediate completion function to mux navigators
|
||||
*/
|
||||
void onCompletion(
|
||||
typename ActionT::Result::SharedPtr result,
|
||||
const nav2_behavior_tree::BtStatus final_bt_status)
|
||||
{
|
||||
plugin_muxer_->stopNavigating(getName());
|
||||
goalCompleted(result, final_bt_status);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief A callback to be called when a new goal is received by the BT action server
|
||||
* Can be used to check if goal is valid and put values on
|
||||
* the blackboard which depend on the received goal
|
||||
*/
|
||||
virtual bool goalReceived(typename ActionT::Goal::ConstSharedPtr goal) = 0;
|
||||
|
||||
/**
|
||||
* @brief A callback that defines execution that happens on one iteration through the BT
|
||||
* Can be used to publish action feedback
|
||||
*/
|
||||
virtual void onLoop() = 0;
|
||||
|
||||
/**
|
||||
* @brief A callback that is called when a preempt is requested
|
||||
*/
|
||||
virtual void onPreempt(typename ActionT::Goal::ConstSharedPtr goal) = 0;
|
||||
|
||||
/**
|
||||
* @brief A callback that is called when a the action is completed; Can fill in
|
||||
* action result message or indicate that this action is done.
|
||||
*/
|
||||
virtual void goalCompleted(
|
||||
typename ActionT::Result::SharedPtr result,
|
||||
const nav2_behavior_tree::BtStatus final_bt_status) = 0;
|
||||
|
||||
/**
|
||||
* @param Method to configure resources.
|
||||
*/
|
||||
virtual bool configure(
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr /*node*/,
|
||||
std::shared_ptr<nav2_util::OdomSmoother>/*odom_smoother*/)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Method to cleanup resources.
|
||||
*/
|
||||
virtual bool cleanup() {return true;}
|
||||
|
||||
/**
|
||||
* @brief Method to activate any threads involved in execution.
|
||||
*/
|
||||
virtual bool activate() {return true;}
|
||||
|
||||
/**
|
||||
* @brief Method to deactivate and any threads involved in execution.
|
||||
*/
|
||||
virtual bool deactivate() {return true;}
|
||||
|
||||
std::unique_ptr<nav2_behavior_tree::BtActionServer<ActionT>> bt_action_server_;
|
||||
rclcpp::Logger logger_{rclcpp::get_logger("Navigator")};
|
||||
rclcpp::Clock::SharedPtr clock_;
|
||||
FeedbackUtils feedback_utils_;
|
||||
NavigatorMuxer * plugin_muxer_;
|
||||
};
|
||||
|
||||
} // namespace nav2_bt_navigator
|
||||
|
||||
#endif // NAV2_BT_NAVIGATOR__NAVIGATOR_HPP_
|
||||
+120
@@ -0,0 +1,120 @@
|
||||
// Copyright (c) 2021 Samsung Research
|
||||
//
|
||||
// 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_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_THROUGH_POSES_HPP_
|
||||
#define NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_THROUGH_POSES_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav2_bt_navigator/navigator.hpp"
|
||||
#include "nav2_msgs/action/navigate_through_poses.hpp"
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
#include "nav2_util/odometry_utils.hpp"
|
||||
|
||||
namespace nav2_bt_navigator
|
||||
{
|
||||
|
||||
/**
|
||||
* @class NavigateThroughPosesNavigator
|
||||
* @brief A navigator for navigating to a a bunch of intermediary poses
|
||||
*/
|
||||
class NavigateThroughPosesNavigator
|
||||
: public nav2_bt_navigator::Navigator<nav2_msgs::action::NavigateThroughPoses>
|
||||
{
|
||||
public:
|
||||
using ActionT = nav2_msgs::action::NavigateThroughPoses;
|
||||
typedef std::vector<geometry_msgs::msg::PoseStamped> Goals;
|
||||
|
||||
/**
|
||||
* @brief A constructor for NavigateThroughPosesNavigator
|
||||
*/
|
||||
NavigateThroughPosesNavigator()
|
||||
: Navigator() {}
|
||||
|
||||
/**
|
||||
* @brief A configure state transition to configure navigator's state
|
||||
* @param node Weakptr to the lifecycle node
|
||||
* @param odom_smoother Object to get current smoothed robot's speed
|
||||
*/
|
||||
bool configure(
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr node,
|
||||
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother) override;
|
||||
|
||||
/**
|
||||
* @brief Get action name for this navigator
|
||||
* @return string Name of action server
|
||||
*/
|
||||
std::string getName() override {return std::string("navigate_through_poses");}
|
||||
|
||||
/**
|
||||
* @brief Get navigator's default BT
|
||||
* @param node WeakPtr to the lifecycle node
|
||||
* @return string Filepath to default XML
|
||||
*/
|
||||
std::string getDefaultBTFilepath(rclcpp_lifecycle::LifecycleNode::WeakPtr node) override;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief A callback to be called when a new goal is received by the BT action server
|
||||
* Can be used to check if goal is valid and put values on
|
||||
* the blackboard which depend on the received goal
|
||||
* @param goal Action template's goal message
|
||||
* @return bool if goal was received successfully to be processed
|
||||
*/
|
||||
bool goalReceived(ActionT::Goal::ConstSharedPtr goal) override;
|
||||
|
||||
/**
|
||||
* @brief A callback that defines execution that happens on one iteration through the BT
|
||||
* Can be used to publish action feedback
|
||||
*/
|
||||
void onLoop() override;
|
||||
|
||||
/**
|
||||
* @brief A callback that is called when a preempt is requested
|
||||
*/
|
||||
void onPreempt(ActionT::Goal::ConstSharedPtr goal) override;
|
||||
|
||||
/**
|
||||
* @brief A callback that is called when a the action is completed, can fill in
|
||||
* action result message or indicate that this action is done.
|
||||
* @param result Action template result message to populate
|
||||
* @param final_bt_status Resulting status of the behavior tree execution that may be
|
||||
* referenced while populating the result.
|
||||
*/
|
||||
void goalCompleted(
|
||||
typename ActionT::Result::SharedPtr result,
|
||||
const nav2_behavior_tree::BtStatus final_bt_status) override;
|
||||
|
||||
/**
|
||||
* @brief Goal pose initialization on the blackboard
|
||||
*/
|
||||
void initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal);
|
||||
|
||||
rclcpp::Time start_time_;
|
||||
std::string goals_blackboard_id_;
|
||||
std::string path_blackboard_id_;
|
||||
|
||||
// Odometry smoother object
|
||||
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
|
||||
};
|
||||
|
||||
} // namespace nav2_bt_navigator
|
||||
|
||||
#endif // NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_THROUGH_POSES_HPP_
|
||||
+136
@@ -0,0 +1,136 @@
|
||||
// Copyright (c) 2021 Samsung Research
|
||||
//
|
||||
// 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_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_TO_POSE_HPP_
|
||||
#define NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_TO_POSE_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav2_bt_navigator/navigator.hpp"
|
||||
#include "nav2_msgs/action/navigate_to_pose.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
#include "nav2_util/odometry_utils.hpp"
|
||||
|
||||
namespace nav2_bt_navigator
|
||||
{
|
||||
|
||||
/**
|
||||
* @class NavigateToPoseNavigator
|
||||
* @brief A navigator for navigating to a specified pose
|
||||
*/
|
||||
class NavigateToPoseNavigator
|
||||
: public nav2_bt_navigator::Navigator<nav2_msgs::action::NavigateToPose>
|
||||
{
|
||||
public:
|
||||
using ActionT = nav2_msgs::action::NavigateToPose;
|
||||
|
||||
/**
|
||||
* @brief A constructor for NavigateToPoseNavigator
|
||||
*/
|
||||
NavigateToPoseNavigator()
|
||||
: Navigator() {}
|
||||
|
||||
/**
|
||||
* @brief A configure state transition to configure navigator's state
|
||||
* @param node Weakptr to the lifecycle node
|
||||
* @param odom_smoother Object to get current smoothed robot's speed
|
||||
*/
|
||||
bool configure(
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr node,
|
||||
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother) override;
|
||||
|
||||
/**
|
||||
* @brief A cleanup state transition to remove memory allocated
|
||||
*/
|
||||
bool cleanup() override;
|
||||
|
||||
/**
|
||||
* @brief A subscription and callback to handle the topic-based goal published
|
||||
* from rviz
|
||||
* @param pose Pose received via atopic
|
||||
*/
|
||||
void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose);
|
||||
|
||||
/**
|
||||
* @brief Get action name for this navigator
|
||||
* @return string Name of action server
|
||||
*/
|
||||
std::string getName() override {return std::string("navigate_to_pose");}
|
||||
|
||||
/**
|
||||
* @brief Get navigator's default BT
|
||||
* @param node WeakPtr to the lifecycle node
|
||||
* @return string Filepath to default XML
|
||||
*/
|
||||
std::string getDefaultBTFilepath(rclcpp_lifecycle::LifecycleNode::WeakPtr node) override;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief A callback to be called when a new goal is received by the BT action server
|
||||
* Can be used to check if goal is valid and put values on
|
||||
* the blackboard which depend on the received goal
|
||||
* @param goal Action template's goal message
|
||||
* @return bool if goal was received successfully to be processed
|
||||
*/
|
||||
bool goalReceived(ActionT::Goal::ConstSharedPtr goal) override;
|
||||
|
||||
/**
|
||||
* @brief A callback that defines execution that happens on one iteration through the BT
|
||||
* Can be used to publish action feedback
|
||||
*/
|
||||
void onLoop() override;
|
||||
|
||||
/**
|
||||
* @brief A callback that is called when a preempt is requested
|
||||
*/
|
||||
void onPreempt(ActionT::Goal::ConstSharedPtr goal) override;
|
||||
|
||||
/**
|
||||
* @brief A callback that is called when a the action is completed, can fill in
|
||||
* action result message or indicate that this action is done.
|
||||
* @param result Action template result message to populate
|
||||
* @param final_bt_status Resulting status of the behavior tree execution that may be
|
||||
* referenced while populating the result.
|
||||
*/
|
||||
void goalCompleted(
|
||||
typename ActionT::Result::SharedPtr result,
|
||||
const nav2_behavior_tree::BtStatus final_bt_status) override;
|
||||
|
||||
/**
|
||||
* @brief Goal pose initialization on the blackboard
|
||||
* @param goal Action template's goal message to process
|
||||
*/
|
||||
void initializeGoalPose(ActionT::Goal::ConstSharedPtr goal);
|
||||
|
||||
rclcpp::Time start_time_;
|
||||
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
|
||||
rclcpp_action::Client<ActionT>::SharedPtr self_client_;
|
||||
|
||||
std::string goal_blackboard_id_;
|
||||
std::string path_blackboard_id_;
|
||||
|
||||
// Odometry smoother object
|
||||
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
|
||||
};
|
||||
|
||||
} // namespace nav2_bt_navigator
|
||||
|
||||
#endif // NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_TO_POSE_HPP_
|
||||
@@ -0,0 +1,45 @@
|
||||
<?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_bt_navigator</name>
|
||||
<version>1.1.18</version>
|
||||
<description>TODO</description>
|
||||
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<depend>tf2_ros</depend>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<build_depend>nav2_common</build_depend>
|
||||
<build_depend>rclcpp</build_depend>
|
||||
<build_depend>rclcpp_action</build_depend>
|
||||
<build_depend>rclcpp_lifecycle</build_depend>
|
||||
<build_depend>nav2_behavior_tree</build_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>nav2_msgs</build_depend>
|
||||
<build_depend>behaviortree_cpp_v3</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>std_srvs</build_depend>
|
||||
<build_depend>nav2_util</build_depend>
|
||||
<build_depend>nav2_core</build_depend>
|
||||
|
||||
<exec_depend>behaviortree_cpp_v3</exec_depend>
|
||||
<exec_depend>rclcpp</exec_depend>
|
||||
<exec_depend>rclcpp_action</exec_depend>
|
||||
<exec_depend>rclcpp_lifecycle</exec_depend>
|
||||
<exec_depend>nav2_behavior_tree</exec_depend>
|
||||
<exec_depend>nav_msgs</exec_depend>
|
||||
<exec_depend>nav2_msgs</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>nav2_util</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>nav2_core</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,214 @@
|
||||
// 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 "nav2_bt_navigator/bt_navigator.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <set>
|
||||
#include <limits>
|
||||
#include <vector>
|
||||
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
#include "nav2_behavior_tree/bt_conversions.hpp"
|
||||
|
||||
namespace nav2_bt_navigator
|
||||
{
|
||||
|
||||
BtNavigator::BtNavigator(rclcpp::NodeOptions options)
|
||||
: nav2_util::LifecycleNode("bt_navigator", "",
|
||||
options.automatically_declare_parameters_from_overrides(true))
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Creating");
|
||||
|
||||
const std::vector<std::string> plugin_libs = {
|
||||
"nav2_compute_path_to_pose_action_bt_node",
|
||||
"nav2_compute_path_through_poses_action_bt_node",
|
||||
"nav2_smooth_path_action_bt_node",
|
||||
"nav2_follow_path_action_bt_node",
|
||||
"nav2_spin_action_bt_node",
|
||||
"nav2_wait_action_bt_node",
|
||||
"nav2_assisted_teleop_action_bt_node",
|
||||
"nav2_back_up_action_bt_node",
|
||||
"nav2_drive_on_heading_bt_node",
|
||||
"nav2_clear_costmap_service_bt_node",
|
||||
"nav2_is_stuck_condition_bt_node",
|
||||
"nav2_goal_reached_condition_bt_node",
|
||||
"nav2_initial_pose_received_condition_bt_node",
|
||||
"nav2_goal_updated_condition_bt_node",
|
||||
"nav2_globally_updated_goal_condition_bt_node",
|
||||
"nav2_is_path_valid_condition_bt_node",
|
||||
"nav2_reinitialize_global_localization_service_bt_node",
|
||||
"nav2_rate_controller_bt_node",
|
||||
"nav2_distance_controller_bt_node",
|
||||
"nav2_speed_controller_bt_node",
|
||||
"nav2_truncate_path_action_bt_node",
|
||||
"nav2_truncate_path_local_action_bt_node",
|
||||
"nav2_goal_updater_node_bt_node",
|
||||
"nav2_recovery_node_bt_node",
|
||||
"nav2_pipeline_sequence_bt_node",
|
||||
"nav2_round_robin_node_bt_node",
|
||||
"nav2_transform_available_condition_bt_node",
|
||||
"nav2_time_expired_condition_bt_node",
|
||||
"nav2_path_expiring_timer_condition",
|
||||
"nav2_distance_traveled_condition_bt_node",
|
||||
"nav2_single_trigger_bt_node",
|
||||
"nav2_goal_updated_controller_bt_node",
|
||||
"nav2_is_battery_low_condition_bt_node",
|
||||
"nav2_navigate_through_poses_action_bt_node",
|
||||
"nav2_navigate_to_pose_action_bt_node",
|
||||
"nav2_remove_passed_goals_action_bt_node",
|
||||
"nav2_planner_selector_bt_node",
|
||||
"nav2_controller_selector_bt_node",
|
||||
"nav2_goal_checker_selector_bt_node",
|
||||
"nav2_controller_cancel_bt_node",
|
||||
"nav2_path_longer_on_approach_bt_node",
|
||||
"nav2_wait_cancel_bt_node",
|
||||
"nav2_spin_cancel_bt_node",
|
||||
"nav2_assisted_teleop_cancel_bt_node",
|
||||
"nav2_back_up_cancel_bt_node",
|
||||
"nav2_drive_on_heading_cancel_bt_node",
|
||||
"nav2_is_battery_charging_condition_bt_node"
|
||||
};
|
||||
|
||||
declare_parameter_if_not_declared(
|
||||
this, "plugin_lib_names", rclcpp::ParameterValue(plugin_libs));
|
||||
declare_parameter_if_not_declared(
|
||||
this, "transform_tolerance", rclcpp::ParameterValue(0.1));
|
||||
declare_parameter_if_not_declared(
|
||||
this, "global_frame", rclcpp::ParameterValue(std::string("map")));
|
||||
declare_parameter_if_not_declared(
|
||||
this, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link")));
|
||||
declare_parameter_if_not_declared(
|
||||
this, "odom_topic", rclcpp::ParameterValue(std::string("odom")));
|
||||
}
|
||||
|
||||
BtNavigator::~BtNavigator()
|
||||
{
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn
|
||||
BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Configuring");
|
||||
|
||||
tf_ = std::make_shared<tf2_ros::Buffer>(get_clock());
|
||||
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
|
||||
get_node_base_interface(), get_node_timers_interface());
|
||||
tf_->setCreateTimerInterface(timer_interface);
|
||||
tf_->setUsingDedicatedThread(true);
|
||||
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_, this, false);
|
||||
|
||||
global_frame_ = get_parameter("global_frame").as_string();
|
||||
robot_frame_ = get_parameter("robot_base_frame").as_string();
|
||||
transform_tolerance_ = get_parameter("transform_tolerance").as_double();
|
||||
odom_topic_ = get_parameter("odom_topic").as_string();
|
||||
|
||||
// Libraries to pull plugins (BT Nodes) from
|
||||
auto plugin_lib_names = get_parameter("plugin_lib_names").as_string_array();
|
||||
|
||||
pose_navigator_ = std::make_unique<nav2_bt_navigator::NavigateToPoseNavigator>();
|
||||
poses_navigator_ = std::make_unique<nav2_bt_navigator::NavigateThroughPosesNavigator>();
|
||||
|
||||
nav2_bt_navigator::FeedbackUtils feedback_utils;
|
||||
feedback_utils.tf = tf_;
|
||||
feedback_utils.global_frame = global_frame_;
|
||||
feedback_utils.robot_frame = robot_frame_;
|
||||
feedback_utils.transform_tolerance = transform_tolerance_;
|
||||
|
||||
// Odometry smoother object for getting current speed
|
||||
odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>(shared_from_this(), 0.3, odom_topic_);
|
||||
|
||||
if (!pose_navigator_->on_configure(
|
||||
shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_, odom_smoother_))
|
||||
{
|
||||
return nav2_util::CallbackReturn::FAILURE;
|
||||
}
|
||||
|
||||
if (!poses_navigator_->on_configure(
|
||||
shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_, odom_smoother_))
|
||||
{
|
||||
return nav2_util::CallbackReturn::FAILURE;
|
||||
}
|
||||
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn
|
||||
BtNavigator::on_activate(const rclcpp_lifecycle::State & /*state*/)
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Activating");
|
||||
|
||||
if (!poses_navigator_->on_activate() || !pose_navigator_->on_activate()) {
|
||||
return nav2_util::CallbackReturn::FAILURE;
|
||||
}
|
||||
|
||||
// create bond connection
|
||||
createBond();
|
||||
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn
|
||||
BtNavigator::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Deactivating");
|
||||
|
||||
if (!poses_navigator_->on_deactivate() || !pose_navigator_->on_deactivate()) {
|
||||
return nav2_util::CallbackReturn::FAILURE;
|
||||
}
|
||||
|
||||
// destroy bond connection
|
||||
destroyBond();
|
||||
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn
|
||||
BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Cleaning up");
|
||||
|
||||
// Reset the listener before the buffer
|
||||
tf_listener_.reset();
|
||||
tf_.reset();
|
||||
|
||||
if (!poses_navigator_->on_cleanup() || !pose_navigator_->on_cleanup()) {
|
||||
return nav2_util::CallbackReturn::FAILURE;
|
||||
}
|
||||
|
||||
poses_navigator_.reset();
|
||||
pose_navigator_.reset();
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Completed Cleaning up");
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn
|
||||
BtNavigator::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Shutting down");
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
} // namespace nav2_bt_navigator
|
||||
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
// Register the component with class_loader.
|
||||
// This acts as a sort of entry point, allowing the component to be discoverable when its library
|
||||
// is being loaded into a running process.
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(nav2_bt_navigator::BtNavigator)
|
||||
@@ -0,0 +1,28 @@
|
||||
// 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 "nav2_bt_navigator/bt_navigator.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<nav2_bt_navigator::BtNavigator>();
|
||||
rclcpp::spin(node->get_node_base_interface());
|
||||
rclcpp::shutdown();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,219 @@
|
||||
// Copyright (c) 2021 Samsung Research
|
||||
//
|
||||
// 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 <set>
|
||||
#include <memory>
|
||||
#include <limits>
|
||||
#include "nav2_bt_navigator/navigators/navigate_through_poses.hpp"
|
||||
|
||||
namespace nav2_bt_navigator
|
||||
{
|
||||
|
||||
bool
|
||||
NavigateThroughPosesNavigator::configure(
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node,
|
||||
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother)
|
||||
{
|
||||
start_time_ = rclcpp::Time(0);
|
||||
auto node = parent_node.lock();
|
||||
|
||||
if (!node->has_parameter("goals_blackboard_id")) {
|
||||
node->declare_parameter("goals_blackboard_id", std::string("goals"));
|
||||
}
|
||||
|
||||
goals_blackboard_id_ = node->get_parameter("goals_blackboard_id").as_string();
|
||||
|
||||
if (!node->has_parameter("path_blackboard_id")) {
|
||||
node->declare_parameter("path_blackboard_id", std::string("path"));
|
||||
}
|
||||
|
||||
path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string();
|
||||
|
||||
// Odometry smoother object for getting current speed
|
||||
odom_smoother_ = odom_smoother;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
std::string
|
||||
NavigateThroughPosesNavigator::getDefaultBTFilepath(
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node)
|
||||
{
|
||||
std::string default_bt_xml_filename;
|
||||
auto node = parent_node.lock();
|
||||
|
||||
if (!node->has_parameter("default_nav_through_poses_bt_xml")) {
|
||||
std::string pkg_share_dir =
|
||||
ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
|
||||
node->declare_parameter<std::string>(
|
||||
"default_nav_through_poses_bt_xml",
|
||||
pkg_share_dir +
|
||||
"/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml");
|
||||
}
|
||||
|
||||
node->get_parameter("default_nav_through_poses_bt_xml", default_bt_xml_filename);
|
||||
|
||||
return default_bt_xml_filename;
|
||||
}
|
||||
|
||||
bool
|
||||
NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
|
||||
{
|
||||
auto bt_xml_filename = goal->behavior_tree;
|
||||
|
||||
if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) {
|
||||
RCLCPP_ERROR(
|
||||
logger_, "Error loading XML file: %s. Navigation canceled.",
|
||||
bt_xml_filename.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
initializeGoalPoses(goal);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
NavigateThroughPosesNavigator::goalCompleted(
|
||||
typename ActionT::Result::SharedPtr /*result*/,
|
||||
const nav2_behavior_tree::BtStatus /*final_bt_status*/)
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
NavigateThroughPosesNavigator::onLoop()
|
||||
{
|
||||
using namespace nav2_util::geometry_utils; // NOLINT
|
||||
|
||||
// action server feedback (pose, duration of task,
|
||||
// number of recoveries, and distance remaining to goal, etc)
|
||||
auto feedback_msg = std::make_shared<ActionT::Feedback>();
|
||||
|
||||
auto blackboard = bt_action_server_->getBlackboard();
|
||||
|
||||
Goals goal_poses;
|
||||
blackboard->get<Goals>(goals_blackboard_id_, goal_poses);
|
||||
|
||||
if (goal_poses.size() == 0) {
|
||||
bt_action_server_->publishFeedback(feedback_msg);
|
||||
return;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PoseStamped current_pose;
|
||||
nav2_util::getCurrentPose(
|
||||
current_pose, *feedback_utils_.tf,
|
||||
feedback_utils_.global_frame, feedback_utils_.robot_frame,
|
||||
feedback_utils_.transform_tolerance);
|
||||
|
||||
try {
|
||||
// Get current path points
|
||||
nav_msgs::msg::Path current_path;
|
||||
blackboard->get<nav_msgs::msg::Path>(path_blackboard_id_, current_path);
|
||||
|
||||
// Find the closest pose to current pose on global path
|
||||
auto find_closest_pose_idx =
|
||||
[¤t_pose, ¤t_path]() {
|
||||
size_t closest_pose_idx = 0;
|
||||
double curr_min_dist = std::numeric_limits<double>::max();
|
||||
for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) {
|
||||
double curr_dist = nav2_util::geometry_utils::euclidean_distance(
|
||||
current_pose, current_path.poses[curr_idx]);
|
||||
if (curr_dist < curr_min_dist) {
|
||||
curr_min_dist = curr_dist;
|
||||
closest_pose_idx = curr_idx;
|
||||
}
|
||||
}
|
||||
return closest_pose_idx;
|
||||
};
|
||||
|
||||
// Calculate distance on the path
|
||||
double distance_remaining =
|
||||
nav2_util::geometry_utils::calculate_path_length(current_path, find_closest_pose_idx());
|
||||
|
||||
// Default value for time remaining
|
||||
rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds(0.0);
|
||||
|
||||
// Get current speed
|
||||
geometry_msgs::msg::Twist current_odom = odom_smoother_->getTwist();
|
||||
double current_linear_speed = std::hypot(current_odom.linear.x, current_odom.linear.y);
|
||||
|
||||
// Calculate estimated time taken to goal if speed is higher than 1cm/s
|
||||
// and at least 10cm to go
|
||||
if ((std::abs(current_linear_speed) > 0.01) && (distance_remaining > 0.1)) {
|
||||
estimated_time_remaining =
|
||||
rclcpp::Duration::from_seconds(distance_remaining / std::abs(current_linear_speed));
|
||||
}
|
||||
|
||||
feedback_msg->distance_remaining = distance_remaining;
|
||||
feedback_msg->estimated_time_remaining = estimated_time_remaining;
|
||||
} catch (...) {
|
||||
// Ignore
|
||||
}
|
||||
|
||||
int recovery_count = 0;
|
||||
blackboard->get<int>("number_recoveries", recovery_count);
|
||||
feedback_msg->number_of_recoveries = recovery_count;
|
||||
feedback_msg->current_pose = current_pose;
|
||||
feedback_msg->navigation_time = clock_->now() - start_time_;
|
||||
feedback_msg->number_of_poses_remaining = goal_poses.size();
|
||||
|
||||
bt_action_server_->publishFeedback(feedback_msg);
|
||||
}
|
||||
|
||||
void
|
||||
NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
|
||||
{
|
||||
RCLCPP_INFO(logger_, "Received goal preemption request");
|
||||
|
||||
if (goal->behavior_tree == bt_action_server_->getCurrentBTFilename() ||
|
||||
(goal->behavior_tree.empty() &&
|
||||
bt_action_server_->getCurrentBTFilename() == bt_action_server_->getDefaultBTFilename()))
|
||||
{
|
||||
// if pending goal requests the same BT as the current goal, accept the pending goal
|
||||
// if pending goal has an empty behavior_tree field, it requests the default BT file
|
||||
// accept the pending goal if the current goal is running the default BT file
|
||||
initializeGoalPoses(bt_action_server_->acceptPendingGoal());
|
||||
} else {
|
||||
RCLCPP_WARN(
|
||||
logger_,
|
||||
"Preemption request was rejected since the requested BT XML file is not the same "
|
||||
"as the one that the current goal is executing. Preemption with a new BT is invalid "
|
||||
"since it would require cancellation of the previous goal instead of true preemption."
|
||||
"\nCancel the current goal and send a new action request if you want to use a "
|
||||
"different BT XML file. For now, continuing to track the last goal until completion.");
|
||||
bt_action_server_->terminatePendingGoal();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal)
|
||||
{
|
||||
if (goal->poses.size() > 0) {
|
||||
RCLCPP_INFO(
|
||||
logger_, "Begin navigating from current location through %zu poses to (%.2f, %.2f)",
|
||||
goal->poses.size(), goal->poses.back().pose.position.x, goal->poses.back().pose.position.y);
|
||||
}
|
||||
|
||||
// Reset state for new action feedback
|
||||
start_time_ = clock_->now();
|
||||
auto blackboard = bt_action_server_->getBlackboard();
|
||||
blackboard->set<int>("number_recoveries", 0); // NOLINT
|
||||
|
||||
// Update the goal pose on the blackboard
|
||||
blackboard->set<Goals>(goals_blackboard_id_, goal->poses);
|
||||
}
|
||||
|
||||
} // namespace nav2_bt_navigator
|
||||
@@ -0,0 +1,235 @@
|
||||
// Copyright (c) 2021 Samsung Research
|
||||
//
|
||||
// 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 <set>
|
||||
#include <memory>
|
||||
#include <limits>
|
||||
#include "nav2_bt_navigator/navigators/navigate_to_pose.hpp"
|
||||
|
||||
namespace nav2_bt_navigator
|
||||
{
|
||||
|
||||
bool
|
||||
NavigateToPoseNavigator::configure(
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node,
|
||||
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother)
|
||||
{
|
||||
start_time_ = rclcpp::Time(0);
|
||||
auto node = parent_node.lock();
|
||||
|
||||
if (!node->has_parameter("goal_blackboard_id")) {
|
||||
node->declare_parameter("goal_blackboard_id", std::string("goal"));
|
||||
}
|
||||
|
||||
goal_blackboard_id_ = node->get_parameter("goal_blackboard_id").as_string();
|
||||
|
||||
if (!node->has_parameter("path_blackboard_id")) {
|
||||
node->declare_parameter("path_blackboard_id", std::string("path"));
|
||||
}
|
||||
|
||||
path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string();
|
||||
|
||||
// Odometry smoother object for getting current speed
|
||||
odom_smoother_ = odom_smoother;
|
||||
|
||||
self_client_ = rclcpp_action::create_client<ActionT>(node, getName());
|
||||
|
||||
goal_sub_ = node->create_subscription<geometry_msgs::msg::PoseStamped>(
|
||||
"goal_pose",
|
||||
rclcpp::SystemDefaultsQoS(),
|
||||
std::bind(&NavigateToPoseNavigator::onGoalPoseReceived, this, std::placeholders::_1));
|
||||
return true;
|
||||
}
|
||||
|
||||
std::string
|
||||
NavigateToPoseNavigator::getDefaultBTFilepath(
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node)
|
||||
{
|
||||
std::string default_bt_xml_filename;
|
||||
auto node = parent_node.lock();
|
||||
|
||||
if (!node->has_parameter("default_nav_to_pose_bt_xml")) {
|
||||
std::string pkg_share_dir =
|
||||
ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
|
||||
node->declare_parameter<std::string>(
|
||||
"default_nav_to_pose_bt_xml",
|
||||
pkg_share_dir +
|
||||
"/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml");
|
||||
}
|
||||
|
||||
node->get_parameter("default_nav_to_pose_bt_xml", default_bt_xml_filename);
|
||||
|
||||
return default_bt_xml_filename;
|
||||
}
|
||||
|
||||
bool
|
||||
NavigateToPoseNavigator::cleanup()
|
||||
{
|
||||
goal_sub_.reset();
|
||||
self_client_.reset();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool
|
||||
NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
|
||||
{
|
||||
auto bt_xml_filename = goal->behavior_tree;
|
||||
|
||||
if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) {
|
||||
RCLCPP_ERROR(
|
||||
logger_, "BT file not found: %s. Navigation canceled.",
|
||||
bt_xml_filename.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
initializeGoalPose(goal);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
NavigateToPoseNavigator::goalCompleted(
|
||||
typename ActionT::Result::SharedPtr /*result*/,
|
||||
const nav2_behavior_tree::BtStatus /*final_bt_status*/)
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
NavigateToPoseNavigator::onLoop()
|
||||
{
|
||||
// action server feedback (pose, duration of task,
|
||||
// number of recoveries, and distance remaining to goal)
|
||||
auto feedback_msg = std::make_shared<ActionT::Feedback>();
|
||||
|
||||
geometry_msgs::msg::PoseStamped current_pose;
|
||||
nav2_util::getCurrentPose(
|
||||
current_pose, *feedback_utils_.tf,
|
||||
feedback_utils_.global_frame, feedback_utils_.robot_frame,
|
||||
feedback_utils_.transform_tolerance);
|
||||
|
||||
auto blackboard = bt_action_server_->getBlackboard();
|
||||
|
||||
try {
|
||||
// Get current path points
|
||||
nav_msgs::msg::Path current_path;
|
||||
blackboard->get<nav_msgs::msg::Path>(path_blackboard_id_, current_path);
|
||||
|
||||
// Find the closest pose to current pose on global path
|
||||
auto find_closest_pose_idx =
|
||||
[¤t_pose, ¤t_path]() {
|
||||
size_t closest_pose_idx = 0;
|
||||
double curr_min_dist = std::numeric_limits<double>::max();
|
||||
for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) {
|
||||
double curr_dist = nav2_util::geometry_utils::euclidean_distance(
|
||||
current_pose, current_path.poses[curr_idx]);
|
||||
if (curr_dist < curr_min_dist) {
|
||||
curr_min_dist = curr_dist;
|
||||
closest_pose_idx = curr_idx;
|
||||
}
|
||||
}
|
||||
return closest_pose_idx;
|
||||
};
|
||||
|
||||
// Calculate distance on the path
|
||||
double distance_remaining =
|
||||
nav2_util::geometry_utils::calculate_path_length(current_path, find_closest_pose_idx());
|
||||
|
||||
// Default value for time remaining
|
||||
rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds(0.0);
|
||||
|
||||
// Get current speed
|
||||
geometry_msgs::msg::Twist current_odom = odom_smoother_->getTwist();
|
||||
double current_linear_speed = std::hypot(current_odom.linear.x, current_odom.linear.y);
|
||||
|
||||
// Calculate estimated time taken to goal if speed is higher than 1cm/s
|
||||
// and at least 10cm to go
|
||||
if ((std::abs(current_linear_speed) > 0.01) && (distance_remaining > 0.1)) {
|
||||
estimated_time_remaining =
|
||||
rclcpp::Duration::from_seconds(distance_remaining / std::abs(current_linear_speed));
|
||||
}
|
||||
|
||||
feedback_msg->distance_remaining = distance_remaining;
|
||||
feedback_msg->estimated_time_remaining = estimated_time_remaining;
|
||||
} catch (...) {
|
||||
// Ignore
|
||||
}
|
||||
|
||||
int recovery_count = 0;
|
||||
blackboard->get<int>("number_recoveries", recovery_count);
|
||||
feedback_msg->number_of_recoveries = recovery_count;
|
||||
feedback_msg->current_pose = current_pose;
|
||||
feedback_msg->navigation_time = clock_->now() - start_time_;
|
||||
|
||||
bt_action_server_->publishFeedback(feedback_msg);
|
||||
}
|
||||
|
||||
void
|
||||
NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
|
||||
{
|
||||
RCLCPP_INFO(logger_, "Received goal preemption request");
|
||||
|
||||
if (goal->behavior_tree == bt_action_server_->getCurrentBTFilename() ||
|
||||
(goal->behavior_tree.empty() &&
|
||||
bt_action_server_->getCurrentBTFilename() == bt_action_server_->getDefaultBTFilename()))
|
||||
{
|
||||
// if pending goal requests the same BT as the current goal, accept the pending goal
|
||||
// if pending goal has an empty behavior_tree field, it requests the default BT file
|
||||
// accept the pending goal if the current goal is running the default BT file
|
||||
initializeGoalPose(bt_action_server_->acceptPendingGoal());
|
||||
} else {
|
||||
RCLCPP_WARN(
|
||||
logger_,
|
||||
"Preemption request was rejected since the requested BT XML file is not the same "
|
||||
"as the one that the current goal is executing. Preemption with a new BT is invalid "
|
||||
"since it would require cancellation of the previous goal instead of true preemption."
|
||||
"\nCancel the current goal and send a new action request if you want to use a "
|
||||
"different BT XML file. For now, continuing to track the last goal until completion.");
|
||||
bt_action_server_->terminatePendingGoal();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
|
||||
{
|
||||
geometry_msgs::msg::PoseStamped current_pose;
|
||||
nav2_util::getCurrentPose(
|
||||
current_pose, *feedback_utils_.tf,
|
||||
feedback_utils_.global_frame, feedback_utils_.robot_frame,
|
||||
feedback_utils_.transform_tolerance);
|
||||
|
||||
RCLCPP_INFO(
|
||||
logger_, "Begin navigating from current location (%.2f, %.2f) to (%.2f, %.2f)",
|
||||
current_pose.pose.position.x, current_pose.pose.position.y,
|
||||
goal->pose.pose.position.x, goal->pose.pose.position.y);
|
||||
|
||||
// Reset state for new action feedback
|
||||
start_time_ = clock_->now();
|
||||
auto blackboard = bt_action_server_->getBlackboard();
|
||||
blackboard->set<int>("number_recoveries", 0); // NOLINT
|
||||
|
||||
// Update the goal pose on the blackboard
|
||||
blackboard->set<geometry_msgs::msg::PoseStamped>(goal_blackboard_id_, goal->pose);
|
||||
}
|
||||
|
||||
void
|
||||
NavigateToPoseNavigator::onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose)
|
||||
{
|
||||
ActionT::Goal goal;
|
||||
goal.pose = *pose;
|
||||
self_client_->async_send_goal(goal);
|
||||
}
|
||||
|
||||
} // namespace nav2_bt_navigator
|
||||
Reference in New Issue
Block a user