Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix deprecated usage of FutureReturnCode::SUCCESS (#2140) #1

Merged
merged 14 commits into from
Jun 23, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 5 additions & 1 deletion doc/parameters/param_list.md
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,10 @@
| Parameter | Default | Description |
| ----------| --------| ------------|
| default_bt_xml_filename | N/A | path to the default behavior tree XML description |
| plugin_lib_names | ["nav2_compute_path_to_pose_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_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_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node"] | list of behavior tree node shared libraries |
| enable_groot_monitoring | True | enable Groot live monitoring of the behavior tree |
| groot_zmq_publisher_port | 1666 | change port of the zmq publisher needed for groot |
| groot_zmq_server_port | 1667 | change port of the zmq server needed for groot |
| plugin_lib_names | ["nav2_compute_path_to_pose_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_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_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_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_distance_traveled_condition_bt_node", "nav2_rotate_action_bt_node", "nav2_translate_action_bt_node", "nav2_is_battery_low_condition_bt_node", "nav2_goal_updater_node_bt_node", "nav2_navigate_to_pose_action_bt_node"] | list of behavior tree node shared libraries |
| transform_tolerance | 0.1 | TF transform tolerance |
| global_frame | "map" | Reference frame |
| robot_base_frame | "base_link" | Robot base frame |
Expand Down Expand Up @@ -237,6 +240,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame
| `<dwb plugin>`.critics | N/A | List of critic plugins to use |
| `<dwb plugin>`.default_critic_namespaces | ["dwb_critics"] | Namespaces to load critics in |
| `<dwb plugin>`.prune_plan | true | Whether to prune the path of old, passed points |
| `<dwb plugin>`.shorten_transformed_plan | true | Determines whether we will pass the full plan on to the critics |
| `<dwb plugin>`.prune_distance | 1.0 | Distance (m) to prune backward until |
| `<dwb plugin>`.debug_trajectory_details | false | Publish debug information |
| `<dwb plugin>`.trajectory_generator_name | "dwb_plugins::StandardTrajectoryGenerator" | Trajectory generator plugin name |
Expand Down
2 changes: 1 addition & 1 deletion nav2_amcl/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_amcl</name>
<version>0.4.5</version>
<version>0.4.7</version>
<description>
<p>
amcl is a probabilistic localization system for a robot moving in
Expand Down
4 changes: 2 additions & 2 deletions nav2_amcl/src/amcl_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -401,7 +401,7 @@ AmclNode::checkLaserReceived()
RCLCPP_WARN(
get_logger(), "Laser scan has not been received"
" (and thus no pose updates have been published)."
" Verify that data is being published on the %s topic.", scan_topic_);
" Verify that data is being published on the %s topic.", scan_topic_.c_str());
return;
}

Expand All @@ -410,7 +410,7 @@ AmclNode::checkLaserReceived()
RCLCPP_WARN(
get_logger(), "No laser scan received (and thus no pose updates have been published) for %f"
" seconds. Verify that data is being published on the %s topic.",
d.nanoseconds() * 1e-9, scan_topic_);
d.nanoseconds() * 1e-9, scan_topic_.c_str());
}
}

Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
// Copyright (c) 2018 Intel Corporation
// Copyright (c) 2020 Florian Gramss
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -22,6 +23,8 @@
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behaviortree_cpp_v3/xml_parsing.h"
#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"


namespace nav2_behavior_tree
{
Expand All @@ -40,28 +43,29 @@ class BehaviorTreeEngine
std::function<bool()> cancelRequested,
std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10));

BT::Tree buildTreeFromText(
BT::Tree createTreeFromText(
const std::string & xml_string,
BT::Blackboard::Ptr blackboard);

BT::Tree createTreeFromFile(
const std::string & file_path,
BT::Blackboard::Ptr blackboard);

void addGrootMonitoring(
BT::Tree * tree,
uint16_t publisher_port,
uint16_t server_port,
uint16_t max_msg_per_second = 25);

void resetGrootMonitor();

// In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state
void haltAllActions(BT::TreeNode * root_node)
{
// this halt signal should propagate through the entire tree.
root_node->halt();

// but, just in case...
auto visitor = [](BT::TreeNode * node) {
if (node->status() == BT::NodeStatus::RUNNING) {
node->halt();
}
};
BT::applyRecursiveVisitor(root_node, visitor);
}
void haltAllActions(BT::TreeNode * root_node);

protected:
// The factory that will be used to dynamically construct the behavior tree
BT::BehaviorTreeFactory factory_;
std::unique_ptr<BT::PublisherZMQ> groot_monitor_;
};

} // namespace nav2_behavior_tree
Expand Down
10 changes: 5 additions & 5 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,11 +36,11 @@ class BtActionNode : public BT::ActionNodeBase
const BT::NodeConfiguration & conf)
: BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name)
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");

// Get the required items from the blackboard
server_timeout_ =
config().blackboard->get<std::chrono::milliseconds>("server_timeout");
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);

// Initialize the input and output messages
Expand Down Expand Up @@ -231,7 +231,7 @@ class BtActionNode : public BT::ActionNodeBase
auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);

if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) !=
rclcpp::executor::FutureReturnCode::SUCCESS)
rclcpp::FutureReturnCode::SUCCESS)
{
throw std::runtime_error("send_goal failed");
}
Expand All @@ -245,9 +245,9 @@ class BtActionNode : public BT::ActionNodeBase
void increment_recovery_count()
{
int recovery_count = 0;
config().blackboard->get<int>("number_recoveries", recovery_count); // NOLINT
config().blackboard->template get<int>("number_recoveries", recovery_count); // NOLINT
recovery_count += 1;
config().blackboard->set<int>("number_recoveries", recovery_count); // NOLINT
config().blackboard->template set<int>("number_recoveries", recovery_count); // NOLINT
}

std::string action_name_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,11 +35,11 @@ class BtServiceNode : public BT::SyncActionNode
const BT::NodeConfiguration & conf)
: BT::SyncActionNode(service_node_name, conf), service_node_name_(service_node_name)
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");

// Get the required items from the blackboard
server_timeout_ =
config().blackboard->get<std::chrono::milliseconds>("server_timeout");
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);

// Now that we have node_ to use, create the service client for this BT service
Expand Down Expand Up @@ -126,9 +126,9 @@ class BtServiceNode : public BT::SyncActionNode
void increment_recovery_count()
{
int recovery_count = 0;
config().blackboard->get<int>("number_recoveries", recovery_count); // NOLINT
config().blackboard->template get<int>("number_recoveries", recovery_count); // NOLINT
recovery_count += 1;
config().blackboard->set<int>("number_recoveries", recovery_count); // NOLINT
config().blackboard->template set<int>("number_recoveries", recovery_count); // NOLINT
}

std::string service_name_, service_node_name_;
Expand Down
2 changes: 2 additions & 0 deletions nav2_behavior_tree/nav2_tree_nodes.xml
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@
<input_port name="parent">Parent frame for transform</input_port>
</Condition>

<Condition ID="GoalUpdated"/>

<!-- ############################### CONTROL NODES ################################ -->
<Control ID="PipelineSequence"/>

Expand Down
2 changes: 1 addition & 1 deletion nav2_behavior_tree/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_behavior_tree</name>
<version>0.4.5</version>
<version>0.4.7</version>
<description>TODO</description>
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
<maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
Expand Down
52 changes: 46 additions & 6 deletions nav2_behavior_tree/src/behavior_tree_engine.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
// Copyright (c) 2018 Intel Corporation
// Copyright (c) 2020 Florian Gramss
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -21,8 +22,6 @@
#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp_v3/utils/shared_library.h"

using namespace std::chrono_literals;

namespace nav2_behavior_tree
{

Expand Down Expand Up @@ -62,13 +61,54 @@ BehaviorTreeEngine::run(
}

BT::Tree
BehaviorTreeEngine::buildTreeFromText(
BehaviorTreeEngine::createTreeFromText(
const std::string & xml_string,
BT::Blackboard::Ptr blackboard)
{
BT::XMLParser p(factory_);
p.loadFromText(xml_string);
return p.instantiateTree(blackboard);
return factory_.createTreeFromText(xml_string, blackboard);
}

BT::Tree
BehaviorTreeEngine::createTreeFromFile(
const std::string & file_path,
BT::Blackboard::Ptr blackboard)
{
return factory_.createTreeFromFile(file_path, blackboard);
}

void
BehaviorTreeEngine::addGrootMonitoring(
BT::Tree * tree,
uint16_t publisher_port,
uint16_t server_port,
uint16_t max_msg_per_second)
{
// This logger publish status changes using ZeroMQ. Used by Groot
groot_monitor_ = std::make_unique<BT::PublisherZMQ>(
*tree, max_msg_per_second, publisher_port,
server_port);
}

void
BehaviorTreeEngine::resetGrootMonitor()
{
groot_monitor_.reset();
}

// In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state
void
BehaviorTreeEngine::haltAllActions(BT::TreeNode * root_node)
{
// this halt signal should propagate through the entire tree.
root_node->halt();

// but, just in case...
auto visitor = [](BT::TreeNode * node) {
if (node->status() == BT::NodeStatus::RUNNING) {
node->halt();
}
};
BT::applyRecursiveVisitor(root_node, visitor);
}

} // namespace nav2_behavior_tree
2 changes: 1 addition & 1 deletion nav2_bringup/bringup/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_bringup</name>
<version>0.4.5</version>
<version>0.4.7</version>
<description>Bringup scripts and configurations for the navigation2 stack</description>
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
Expand Down
2 changes: 1 addition & 1 deletion nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -226,7 +226,7 @@ recoveries_server:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
recovery_plugins: ["spin", "back_up", "wait"]
recovery_plugins: ["spin", "backup", "wait"]
spin:
plugin: "nav2_recoveries/Spin"
backup:
Expand Down
2 changes: 1 addition & 1 deletion nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -226,7 +226,7 @@ recoveries_server:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
recovery_plugins: ["spin", "back_up", "wait"]
recovery_plugins: ["spin", "backup", "wait"]
spin:
plugin: "nav2_recoveries/Spin"
backup:
Expand Down
8 changes: 6 additions & 2 deletions nav2_bringup/bringup/params/nav2_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,9 @@ bt_navigator:
global_frame: map
robot_base_frame: base_link
odom_topic: /odom
enable_groot_monitoring: True
groot_zmq_publisher_port: 1666
groot_zmq_server_port: 1667
default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
Expand Down Expand Up @@ -243,6 +246,7 @@ map_saver:
save_map_timeout: 5000
free_thresh_default: 0.25
occupied_thresh_default: 0.65
map_subscribe_transient_local: False

planner_server:
ros__parameters:
Expand All @@ -264,10 +268,10 @@ recoveries_server:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
recovery_plugins: ["spin", "backup", "wait"]
recovery_plugins: ["spin", "back_up", "wait"]
spin:
plugin: "nav2_recoveries/Spin"
backup:
back_up:
plugin: "nav2_recoveries/BackUp"
wait:
plugin: "nav2_recoveries/Wait"
Expand Down
4 changes: 2 additions & 2 deletions nav2_bringup/bringup/worlds/waffle.model
Original file line number Diff line number Diff line change
Expand Up @@ -266,7 +266,7 @@
<pose>0.0 0.144 0.023 0 0 0</pose>
<geometry>
<mesh>
<uri>model://turtlebot3_waffle/meshes/left_tire.dae</uri>
<uri>model://turtlebot3_waffle/meshes/tire.dae</uri>
<scale>0.001 0.001 0.001</scale>
</mesh>
</geometry>
Expand Down Expand Up @@ -324,7 +324,7 @@
<pose>0.0 -0.144 0.023 0 0 0</pose>
<geometry>
<mesh>
<uri>model://turtlebot3_waffle/meshes/right_tire.dae</uri>
<uri>model://turtlebot3_waffle/meshes/tire.dae</uri>
<scale>0.001 0.001 0.001</scale>
</mesh>
</geometry>
Expand Down
2 changes: 1 addition & 1 deletion nav2_bringup/nav2_gazebo_spawner/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_gazebo_spawner</name>
<version>0.4.5</version>
<version>0.4.7</version>
<description>Package for spawning a robot model into Gazebo for navigation2</description>
<maintainer email="carlos.orduno@intel.com">lkumarbe</maintainer>
<maintainer email="lalit.kumar.begani@intel.com">lkumarbe</maintainer>
Expand Down
3 changes: 2 additions & 1 deletion nav2_bt_navigator/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -82,5 +82,6 @@ if(BUILD_TESTING)
endif()

ament_export_include_directories(include)

ament_export_libraries(${library_name})
ament_export_dependencies(${dependencies})
ament_package()
2 changes: 1 addition & 1 deletion nav2_bt_navigator/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?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>0.4.5</version>
<version>0.4.7</version>
<description>TODO</description>
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
<license>Apache-2.0</license>
Expand Down
Loading