Skip to content

Commit

Permalink
Humble sync 3: Dec 20 (#3334)
Browse files Browse the repository at this point in the history
* remove exec_depend on behaviortree_cpp_v3 (#3279)

* BT Service Node to throw if service was not available in time (#3256)

* throw if service server wasn't available in time

mimic the behavior of the bt action node constructor

* throw if action unavailable in bt cancel action

* use chrono literals namespace

* fix linting errors

* fix code style divergence

* Remove duplicate of nav2_back_up_cancel_bt_node (#3332)

* Remove unused velocity scaling config from example xml (#3330)

Signed-off-by: Borong Yuan <yuanborong@hotmail.com>

Signed-off-by: Borong Yuan <yuanborong@hotmail.com>

* Make mapUpdateLoop() indicator variables to be thread-safe (#3308)

* Ensure that plugin initialization to be called before updating routines (#3307)

* Solve bug when CostmapInfoServer is reactivated (#3292)

* Solve bug when CostmapInfoServer is reactivated

* Implemented smoother selector bt node (#3283)

* Implemented smoother selector bt node

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

* updated copyright in modified file

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

* Add allow_unknown parameter to theta star planner (#3286)

* Add allow unknown parameter to theta star planner

* Add allow unknown parameter to tests

* missing comma

* Change cost of unknown tiles

* Uncrustify

* bump to 1.1.3 for humble sync 3

Signed-off-by: Borong Yuan <yuanborong@hotmail.com>
Signed-off-by: Owen Hooper <17ofh@queensu.ca>
Co-authored-by: Adam Aposhian <aposhian.dev@gmail.com>
Co-authored-by: Erwin Lejeune <erwin.lejeune15@gmail.com>
Co-authored-by: Sven Langner <svenlr@users.noreply.github.com>
Co-authored-by: Borong Yuan <yuanborong@hotmail.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: MartiBolet <43337758+MartiBolet@users.noreply.github.com>
Co-authored-by: Owen Hooper <17ofh@queensu.ca>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
  • Loading branch information
9 people authored Dec 21, 2022
1 parent 0d86469 commit ed7f5ef
Show file tree
Hide file tree
Showing 62 changed files with 765 additions and 75 deletions.
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

0 comments on commit ed7f5ef

Please sign in to comment.