Humble sync 3: Dec 20 (ros-navigation#3334)

* remove exec_depend on behaviortree_cpp_v3 (ros-navigation#3279) * BT Service Node to throw if service was not available in time (ros-navigation#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 (ros-navigation#3332) * Remove unused velocity scaling config from example xml (ros-navigation#3330) Signed-off-by: Borong Yuan <[email protected]> Signed-off-by: Borong Yuan <[email protected]> * Make mapUpdateLoop() indicator variables to be thread-safe (ros-navigation#3308) * Ensure that plugin initialization to be called before updating routines (ros-navigation#3307) * Solve bug when CostmapInfoServer is reactivated (ros-navigation#3292) * Solve bug when CostmapInfoServer is reactivated * Implemented smoother selector bt node (ros-navigation#3283) * Implemented smoother selector bt node Signed-off-by: Owen Hooper <[email protected]> * updated copyright in modified file Signed-off-by: Owen Hooper <[email protected]> Signed-off-by: Owen Hooper <[email protected]> * Add allow_unknown parameter to theta star planner (ros-navigation#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 <[email protected]> Signed-off-by: Owen Hooper <[email protected]> Co-authored-by: Adam Aposhian <[email protected]> Co-authored-by: Erwin Lejeune <[email protected]> Co-authored-by: Sven Langner <[email protected]> Co-authored-by: Borong Yuan <[email protected]> Co-authored-by: Alexey Merzlyakov <[email protected]> Co-authored-by: MartiBolet <[email protected]> Co-authored-by: Owen Hooper <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]>
moss-ag · Mar 4, 2023 · c370103 · c370103
1 parent fa50e02
commit c370103
Show file tree

Hide file tree

Showing 62 changed files with 765 additions and 75 deletions.
diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml
@@ -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

diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
@@ -186,6 +186,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)
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp
@@ -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
@@ -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()));
+    }
   }
 
   /**

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
@@ -17,6 +17,7 @@
 
 #include <string>
 #include <memory>
+#include <chrono>
 
 #include "behaviortree_cpp_v3/action_node.h"
 #include "nav2_util/node_utils.hpp"
@@ -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
@@ -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",

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp
@@ -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_
diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -164,6 +164,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>

diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml
@@ -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="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>
@@ -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>

diff --git a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp
@@ -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");
+}
diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt
@@ -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})