add humble-navigation2
This commit is contained in:
X-lanni
@@ -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