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

Humble sync 3: Dec 20 #3334

Merged
merged 10 commits into from
Dec 21, 2022
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>1.1.3</version>
<version>1.1.4</version>
<description>
<p>
amcl is a probabilistic localization system for a robot moving in
Expand Down
3 changes: 3 additions & 0 deletions nav2_behavior_tree/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -183,6 +183,9 @@ list(APPEND plugin_libs nav2_planner_selector_bt_node)
add_library(nav2_controller_selector_bt_node SHARED plugins/action/controller_selector_node.cpp)
list(APPEND plugin_libs nav2_controller_selector_bt_node)

add_library(nav2_smoother_selector_bt_node SHARED plugins/action/smoother_selector_node.cpp)
list(APPEND plugin_libs nav2_smoother_selector_bt_node)

add_library(nav2_goal_checker_selector_bt_node SHARED plugins/action/goal_checker_selector_node.cpp)
list(APPEND plugin_libs nav2_goal_checker_selector_bt_node)

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@
namespace nav2_behavior_tree
{

using namespace std::chrono_literals; // NOLINT

/**
* @brief Abstract class representing an action for cancelling BT node
* @tparam ActionT Type of action
Expand Down Expand Up @@ -87,7 +89,12 @@ class BtCancelActionNode : public BT::ActionNodeBase

// Make sure the server is actually there before continuing
RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
action_client_->wait_for_action_server();
if (!action_client_->wait_for_action_server(1s)) {
RCLCPP_ERROR(
node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
action_name.c_str());
throw std::runtime_error(std::string("Action server %s not available", action_name.c_str()));
}
}

/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include <string>
#include <memory>
#include <chrono>

#include "behaviortree_cpp_v3/action_node.h"
#include "nav2_util/node_utils.hpp"
Expand All @@ -26,6 +27,8 @@
namespace nav2_behavior_tree
{

using namespace std::chrono_literals; // NOLINT

/**
* @brief Abstract class representing a service based BT node
* @tparam ServiceT Type of service
Expand Down Expand Up @@ -71,7 +74,15 @@ class BtServiceNode : public BT::ActionNodeBase
RCLCPP_DEBUG(
node_->get_logger(), "Waiting for \"%s\" service",
service_name_.c_str());
service_client_->wait_for_service();
if (!service_client_->wait_for_service(1s)) {
RCLCPP_ERROR(
node_->get_logger(), "\"%s\" service server not available after waiting for 1 s",
service_node_name.c_str());
throw std::runtime_error(
std::string(
"Service server %s not available",
service_node_name.c_str()));
}

RCLCPP_DEBUG(
node_->get_logger(), "\"%s\" BtServiceNode initialized",
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,99 @@
// Copyright (c) 2018 Intel Corporation
// Copyright (c) 2020 Pablo Iñigo Blasco
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTHER_SELECTOR_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTHER_SELECTOR_NODE_HPP_

#include <memory>
#include <string>

#include "std_msgs/msg/string.hpp"

#include "behaviortree_cpp_v3/action_node.h"

#include "rclcpp/rclcpp.hpp"

namespace nav2_behavior_tree
{

/**
* @brief The SmootherSelector behavior is used to switch the smoother
* that will be used by the smoother server. It subscribes to a topic "smoother_selector"
* to get the decision about what smoother must be used. It is usually used before of
* the FollowPath. The selected_smoother output port is passed to smoother_id
* input port of the FollowPath
*/
class SmootherSelector : public BT::SyncActionNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::SmootherSelector
*
* @param xml_tag_name Name for the XML tag for this node
* @param conf BT node configuration
*/
SmootherSelector(
const std::string & xml_tag_name,
const BT::NodeConfiguration & conf);

/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts()
{
return {
BT::InputPort<std::string>(
"default_smoother",
"the default smoother to use if there is not any external topic message received."),

BT::InputPort<std::string>(
"topic_name",
"smoother_selector",
"the input topic name to select the smoother"),

BT::OutputPort<std::string>(
"selected_smoother",
"Selected smoother by subscription")
};
}

private:
/**
* @brief Function to perform some user-defined operation on tick
*/
BT::NodeStatus tick() override;

/**
* @brief callback function for the smoother_selector topic
*
* @param msg the message with the id of the smoother_selector
*/
void callbackSmootherSelect(const std_msgs::msg::String::SharedPtr msg);

rclcpp::Subscription<std_msgs::msg::String>::SharedPtr smoother_selector_sub_;

std::string last_selected_smoother_;

rclcpp::Node::SharedPtr node_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;

std::string topic_name_;
};

} // namespace nav2_behavior_tree

#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTHER_SELECTOR_NODE_HPP_
6 changes: 6 additions & 0 deletions nav2_behavior_tree/nav2_tree_nodes.xml
Original file line number Diff line number Diff line change
Expand Up @@ -162,6 +162,12 @@
<output_port name="selected_controller">Name of the selected controller received from the topic subcription</output_port>
</Action>

<Action ID="SmootherSelector">
<input_port name="topic_name">Name of the topic to receive smoother selection commands</input_port>
<input_port name="default_smoother">Default smoother of the smoother selector</input_port>
<output_port name="selected_smoother">Name of the selected smoother received from the topic subcription</output_port>
</Action>

<Action ID="GoalCheckerSelector">
<input_port name="topic_name">Name of the topic to receive goal checker selection commands</input_port>
<input_port name="default_goal_checker">Default goal checker of the controller selector</input_port>
Expand Down
3 changes: 1 addition & 2 deletions 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>1.1.3</version>
<version>1.1.4</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 Expand Up @@ -36,7 +36,6 @@
<exec_depend>rclcpp_action</exec_depend>
<exec_depend>rclcpp_lifecycle</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>behaviortree_cpp_v3</exec_depend>
<exec_depend>builtin_interfaces</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
Expand Down
92 changes: 92 additions & 0 deletions nav2_behavior_tree/plugins/action/smoother_selector_node.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,92 @@
// Copyright (c) 2018 Intel Corporation
// Copyright (c) 2020 Pablo Iñigo Blasco
// Copyright (c) 2022 Owen Hooper
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <string>
#include <memory>

#include "std_msgs/msg/string.hpp"

#include "nav2_behavior_tree/plugins/action/smoother_selector_node.hpp"

#include "rclcpp/rclcpp.hpp"

namespace nav2_behavior_tree
{

using std::placeholders::_1;

SmootherSelector::SmootherSelector(
const std::string & name,
const BT::NodeConfiguration & conf)
: BT::SyncActionNode(name, conf)
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
callback_group_ = node_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false);
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());

getInput("topic_name", topic_name_);

rclcpp::QoS qos(rclcpp::KeepLast(1));
qos.transient_local().reliable();

rclcpp::SubscriptionOptions sub_option;
sub_option.callback_group = callback_group_;
smoother_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
topic_name_,
qos,
std::bind(&SmootherSelector::callbackSmootherSelect, this, _1),
sub_option);
}

BT::NodeStatus SmootherSelector::tick()
{
callback_group_executor_.spin_some();

// This behavior always use the last selected smoother received from the topic input.
// When no input is specified it uses the default smoother.
// If the default smoother is not specified then we work in "required smoother mode":
// In this mode, the behavior returns failure if the smoother selection is not received from
// the topic input.
if (last_selected_smoother_.empty()) {
std::string default_smoother;
getInput("default_smoother", default_smoother);
if (default_smoother.empty()) {
return BT::NodeStatus::FAILURE;
} else {
last_selected_smoother_ = default_smoother;
}
}

setOutput("selected_smoother", last_selected_smoother_);

return BT::NodeStatus::SUCCESS;
}

void
SmootherSelector::callbackSmootherSelect(const std_msgs::msg::String::SharedPtr msg)
{
last_selected_smoother_ = msg->data;
}

} // namespace nav2_behavior_tree

#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<nav2_behavior_tree::SmootherSelector>("SmootherSelector");
}
4 changes: 4 additions & 0 deletions nav2_behavior_tree/test/plugins/action/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,10 @@ ament_add_gtest(test_controller_selector_node test_controller_selector_node.cpp)
target_link_libraries(test_controller_selector_node nav2_controller_selector_bt_node)
ament_target_dependencies(test_controller_selector_node ${dependencies})

ament_add_gtest(test_smoother_selector_node test_smoother_selector_node.cpp)
target_link_libraries(test_smoother_selector_node nav2_smoother_selector_bt_node)
ament_target_dependencies(test_smoother_selector_node ${dependencies})

ament_add_gtest(test_goal_checker_selector_node test_goal_checker_selector_node.cpp)
target_link_libraries(test_goal_checker_selector_node nav2_goal_checker_selector_bt_node)
ament_target_dependencies(test_goal_checker_selector_node ${dependencies})
Loading