Humble-sync

.circleci/config.yml

@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v11\
+              - "<< parameters.key >>-v12\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v11\
+              - "<< parameters.key >>-v12\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v11\
+            key: "<< parameters.key >>-v12\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\
@@ -447,6 +447,7 @@ _environments:
     RCUTILS_LOGGING_BUFFERED_STREAM: "0"
     RCUTILS_LOGGING_USE_STDOUT: "0"
     DEBIAN_FRONTEND: "noninteractive"
+    PYTHONUNBUFFERED: "1"
 
 executors:
   release_exec:

Dockerfile

@@ -43,6 +43,7 @@ RUN echo '\
 APT::Install-Recommends "0";\n\
 APT::Install-Suggests "0";\n\
 ' > /etc/apt/apt.conf.d/01norecommend
+ENV PYTHONUNBUFFERED 1
 
 # install CI dependencies
 ARG RTI_NC_LICENSE_ACCEPTED=yes

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.2</version>
+  <version>1.1.6</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

nav2_amcl/src/amcl_node.cpp

@@ -1158,18 +1158,53 @@ AmclNode::dynamicParametersCallback(
     if (param_type == ParameterType::PARAMETER_DOUBLE) {
       if (param_name == "alpha1") {
         alpha1_ = parameter.as_double();
+        //alpha restricted to be non-negative
+        if (alpha1_ < 0.0) {
+          RCLCPP_WARN(
+            get_logger(), "You've set alpha1 to be negative,"
+            " this isn't allowed, so the alpha1 will be set to be zero.");
+          alpha1_ = 0.0;
+        }
         reinit_odom = true;
       } else if (param_name == "alpha2") {
         alpha2_ = parameter.as_double();
+        //alpha restricted to be non-negative
+        if (alpha2_ < 0.0) {
+          RCLCPP_WARN(
+            get_logger(), "You've set alpha2 to be negative,"
+            " this isn't allowed, so the alpha2 will be set to be zero.");
+          alpha2_ = 0.0;
+        }
         reinit_odom = true;
       } else if (param_name == "alpha3") {
         alpha3_ = parameter.as_double();
+        //alpha restricted to be non-negative
+        if (alpha3_ < 0.0) {
+          RCLCPP_WARN(
+            get_logger(), "You've set alpha3 to be negative,"
+            " this isn't allowed, so the alpha3 will be set to be zero.");
+          alpha3_ = 0.0;
+        }
         reinit_odom = true;
       } else if (param_name == "alpha4") {
         alpha4_ = parameter.as_double();
+        //alpha restricted to be non-negative
+        if (alpha4_ < 0.0) {
+          RCLCPP_WARN(
+            get_logger(), "You've set alpha4 to be negative,"
+            " this isn't allowed, so the alpha4 will be set to be zero.");
+          alpha4_ = 0.0;
+        }
         reinit_odom = true;
       } else if (param_name == "alpha5") {
         alpha5_ = parameter.as_double();
+        //alpha restricted to be non-negative
+        if (alpha5_ < 0.0) {
+          RCLCPP_WARN(
+            get_logger(), "You've set alpha5 to be negative,"
+            " this isn't allowed, so the alpha5 will be set to be zero.");
+          alpha5_ = 0.0;
+        }
         reinit_odom = true;
       } else if (param_name == "beam_skip_distance") {
         beam_skip_distance_ = parameter.as_double();

nav2_behavior_tree/CMakeLists.txt

@@ -71,6 +71,9 @@ list(APPEND plugin_libs nav2_spin_cancel_bt_node)
 add_library(nav2_back_up_cancel_bt_node SHARED plugins/action/back_up_cancel_node.cpp)
 list(APPEND plugin_libs nav2_back_up_cancel_bt_node)
 
+add_library(nav2_assisted_teleop_cancel_bt_node SHARED plugins/action/assisted_teleop_cancel_node.cpp)
+list(APPEND plugin_libs nav2_assisted_teleop_cancel_bt_node)
+
 add_library(nav2_drive_on_heading_cancel_bt_node SHARED plugins/action/drive_on_heading_cancel_node.cpp)
 list(APPEND plugin_libs nav2_drive_on_heading_cancel_bt_node)
 
@@ -92,6 +95,9 @@ list(APPEND plugin_libs nav2_spin_action_bt_node)
 add_library(nav2_wait_action_bt_node SHARED plugins/action/wait_action.cpp)
 list(APPEND plugin_libs nav2_wait_action_bt_node)
 
+add_library(nav2_assisted_teleop_action_bt_node SHARED plugins/action/assisted_teleop_action.cpp)
+list(APPEND plugin_libs nav2_assisted_teleop_action_bt_node)
+
 add_library(nav2_clear_costmap_service_bt_node SHARED plugins/action/clear_costmap_service.cpp)
 list(APPEND plugin_libs nav2_clear_costmap_service_bt_node)
 
@@ -179,6 +185,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)
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -97,7 +97,9 @@ class BtActionNode : public BT::ActionNodeBase
       RCLCPP_ERROR(
         node_->get_logger(), "\"%s\" action server not available after waiting for 20 s",
         action_name.c_str());
-      throw std::runtime_error(std::string("Action server ") + action_name +  std::string(" not available"));
+      throw std::runtime_error(
+              std::string("Action server ") + action_name +
+              std::string(" not available"));
     }
   }
 

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,14 @@ 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 ") + action_name +
+              std::string(" not available"));
+    }
   }
 
   /**

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(std::chrono::seconds(10));
+    if (!service_client_->wait_for_service(10s)) {
+      RCLCPP_ERROR(
+        node_->get_logger(), "\"%s\" service server not available after waiting for 10 s",
+        service_name_.c_str());
+      throw std::runtime_error(
+              std::string(
+                "Service server %s not available",
+                service_name_.c_str()));
+    }
 
     RCLCPP_DEBUG(
       node_->get_logger(), "\"%s\" BtServiceNode initialized",

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp

@@ -0,0 +1,70 @@
+// Copyright (c) 2022 Joshua Wallace
+//
+// 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__ASSISTED_TELEOP_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_ACTION_HPP_
+
+#include <string>
+
+#include "nav2_behavior_tree/bt_action_node.hpp"
+#include "nav2_msgs/action/assisted_teleop.hpp"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::AssistedTeleop
+ */
+class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTeleop>
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::nav2_msgs::action::AssistedTeleop
+   * @param xml_tag_name Name for the XML tag for this node
+   * @param action_name Action name this node creates a client for
+   * @param conf BT node configuration
+   */
+  AssistedTeleopAction(
+    const std::string & xml_tag_name,
+    const std::string & action_name,
+    const BT::NodeConfiguration & conf);
+
+  /**
+   * @brief Function to perform some user-defined operation on tick
+   */
+  void on_tick() override;
+
+  BT::NodeStatus on_aborted() override;
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing basic ports along with node-specific ports
+   */
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts(
+      {
+        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for running assisted teleop"),
+        BT::InputPort<bool>("is_recovery", false, "If true the recovery count will be incremented")
+      });
+  }
+
+private:
+  bool is_recovery_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_ACTION_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp

@@ -0,0 +1,59 @@
+// Copyright (c) 2022 Joshua Wallace
+//
+// 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__ASSISTED_TELEOP_CANCEL_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_CANCEL_NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "nav2_msgs/action/assisted_teleop.hpp"
+
+#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp
+ */
+class AssistedTeleopCancel : public BtCancelActionNode<nav2_msgs::action::AssistedTeleop>
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::BackUpAction
+   * @param xml_tag_name Name for the XML tag for this node
+   * @param action_name Action name this node creates a client for
+   * @param conf BT node configuration
+   */
+  AssistedTeleopCancel(
+    const std::string & xml_tag_name,
+    const std::string & action_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 providedBasicPorts(
+      {
+      });
+  }
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_CANCEL_NODE_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp

@@ -61,6 +61,11 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
    */
   BT::NodeStatus on_cancelled() override;
 
+  /**
+   * \brief Override required by the a BT action. Cancel the action and set the path output
+   */
+  void halt() override;
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports