Skip to content

Commit

Permalink
Merge pull request #1 from ros-planning/foxy-devel
Browse files Browse the repository at this point in the history
Fix deprecated usage of FutureReturnCode::SUCCESS (ros-navigation#2140)
  • Loading branch information
zikprid0 authored Jun 23, 2021
2 parents 3d166b7 + 1160100 commit d9341df
Show file tree
Hide file tree
Showing 87 changed files with 2,290 additions and 382 deletions.
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

0 comments on commit d9341df

Please sign in to comment.