Humble sync 2: Nov 8 (ros-navigation#3274)

* standalone assisted teleop (ros-navigation#2904) * standalone assisted teleop * added in action message * code review * moved to behavior server * added assisted teleop bt node * revert * added bt node for assisted teleop * lint fix * added cancel assisted teleop node * code review * working * cleanup * updated feeback * code review * update compute velocity * cleanup * lint fixes * cleanup * test fix * starting to add tests for assisted teleop * fixed tests * undo * fixed test * is_recovery * adjust abort result based on recovery or not * code review * added preempt velocity * working preempt assisted teleop test * completed assisted teleop tests * code review * undo * code review * remove sleep * topic rename * missing comma * added comma :( * added comma Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Add the support of range sensors to Collision Monitor (ros-navigation#3099) * Support range sensors in Collision Monitor * Adjust README.md * Meet review fixes * Fix ros-navigation#3152: Costmap extend did not include Y component (ros-navigation#3153) * missing nodes added to nav2_tree_nodes.xml (ros-navigation#3155) * Change deprecated ceres function (ros-navigation#3158) * Change deprecated function * Update smoother_cost_function.hpp * remove camera_rgb_joint since child frame does not exist (ros-navigation#3162) * bugfix (ros-navigation#3109) deadlock when costmap receives new map (ros-navigation#3145) * bugfix (ros-navigation#3109) deadlock when costmap receives new map Signed-off-by: Daisuke Sato <daisukes@cmu.edu> * introduce map_received_in_update_bounds_ flag to make sure processMap will not be called between updateBounds and updateCosts Signed-off-by: Daisuke Sato <daisukes@cmu.edu> Signed-off-by: Daisuke Sato <daisukes@cmu.edu> * simple command costmap api - first few functions (ros-navigation#3159) * initial commit costmap_2d template Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * finish task A and tested * lint * Update nav2_simple_commander/nav2_simple_commander/costmap_2d.py Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * fix trailing underscores Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Fix missing dependency on nav2_collision_monitor (ros-navigation#3175) * fixed start (ros-navigation#3168) * fixed start * return true * fix tests * Fix velocities comparison for rotation at place case (ros-navigation#3177) * Fix velocities comparison for rotation at place case * Meet review item * Remove unnecessary header * Change the comment * set a empty path on halt (ros-navigation#3178) * set a empty path on halt * fixed issues * remove path reset * fixing * reverting * revert * revert * fixed lint * test fix * uncrusify fix * simple command costmap api - update few functions (ros-navigation#3169) * * add aditional function to costmap_2d.py Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> Updated-by: Jaehun Kim <k9632441@gmail.com> * finish task B * Update nav2_simple_commander/nav2_simple_commander/costmap_2d.py * Update method docs * Remove underscores at parameters and split getCost into getCostXY and getCostIdx * Update method docstrings * lint code & update docstring, remove default value of getCostXY * lint code with pep257 & flake8 * clear names for bt nodes (ros-navigation#3183) * [Smac] check if a node exists before creating (ros-navigation#3195) * check if a node exists before creating * invert logic to group like with like * Update a_star.cpp * fixing benchmarkign for planners (ros-navigation#3202) * [Smac] Robin hood data structure improves performance by 10-15%! (ros-navigation#3201) * adding robin_hood unordered_map * using robin_hood node map * ignore robin_hood file * linting * linting cont. for triple pointers * linting cont. for uncrustify * [RPP] Add parameter to enable/disable collision detection (ros-navigation#3204) * [RPP] Add parameter to enable/disable collision detection * [RPP] Update README * Update waffle.model * add benchmark launch file + instructions (ros-navigation#3218) * removing hypotf from smac planner heuristic computation (ros-navigation#3217) * removing hypotf * swapping to node2d sqrt * complete smac planner tolerances (ros-navigation#3219) * Disable Output Buffering (ros-navigation#3220) To ensure await asyncio prints [Processing: %s]' every 30s as expected * fix majority of python linting errors introduced in python costmap API additions to get CI turning over again (ros-navigation#3223) * fix majority of python linting errors * finish linting * Assisted teleop simple commander (ros-navigation#3198) * add assisted teleop to python api * cleanup * assisted teleop demo * rename * lint * code review * trigger build * flake8 fix * break cashe * moved all v11 to v12 * lint fix * remove package dep * change default time allowance * Costmap Filter enabling service (ros-navigation#3229) * Add enabling service to costmap filters * Add service testcase * Fix comment * Use toggle_filter service name * Add binary flip costmap filter (ros-navigation#3228) * Add binary flip costmap filter * Move transformPose, worldToMask, getMaskData to CostmapFilter * Added default parametrized binary filter state * Switched to std_msgs/msg/Bool.msg * Use arbitrary filter values * Update waffle.model * Update waffle.model * Update test_actions.cpp * odom alpha restriction to avoid overflow caused by user-misconfiguration (ros-navigation#3238) * odom alpha restriction * odom alpha code style * odom alpha code style * odom alpha code style * Update controller server goal checker (ros-navigation#3240) * [FIX] Update controller server goal checker * [FIX] Autoformat code * [FIX] Misplaced tabs. Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> * map-size restriction to avoid overflow and nullptr caused by user-misconfiguration (ros-navigation#3242) * odom alpha restriction * odom alpha code style * odom alpha code style * odom alpha code style * map-size restriction * map-size code style * map-size rejection * map-size codestyle * map-size return false * Add Path Smoothers Benchmarking suite (ros-navigation#3236) * Add Path Smoothers Benchmarking suite * Meet review items * Update tools/smoother_benchmarking/README.md Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Move optional performance patch to the end of README * Fix README Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Fix typo (ros-navigation#3262) * Adding new Nav2 Smoother: Savitzky-Golay Smoother (ros-navigation#3264) * initial prototype of the Savitzky Golay Filter Path Smoother * fixed indexing issue - tested working * updates for filter * adding unit tests for SG-filter smoother * adding lifecycle transitions * Added Line Iterator (ros-navigation#3197) * Added Line Iterator * Updated Line Iterator to a new iteration method * Added the resolution as a parameter/ fixed linting * Added the resolution as a parameter/ fixed linting * Added unittests for the line iterator * Added unittests based on "unittest" package * Fixed __init__.py and rephrased some docstrings * Fixed linting errors * Fixed Linting Errors * Added some unittests and removed some methods * Dummy commit for CircleCI Issue Co-authored-by: Afif Swaidan <afif.swaidan@spexal.com> * bumping to 1.1.3 for release Signed-off-by: Daisuke Sato <daisukes@cmu.edu> Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> Co-authored-by: Joshua Wallace <47819219+jwallace42@users.noreply.github.com> Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Co-authored-by: Abdullah Enes BEDİR <46785079+enesbedir1@users.noreply.github.com> Co-authored-by: Tobias Fischer <info@tobiasfischer.info> Co-authored-by: Tejas Kumar Shastha <tejas.kumar.shastha@ipa.fraunhofer.de> Co-authored-by: Daisuke Sato <43101027+daisukes@users.noreply.github.com> Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Co-authored-by: Lukas Fanta <63977366+fantalukas@users.noreply.github.com> Co-authored-by: Jackson9 <k9632441@gmail.com> Co-authored-by: Ruffin <roxfoxpox@gmail.com> Co-authored-by: Hao-Xuan Song <44140526+Cryst4L9527@users.noreply.github.com> Co-authored-by: Nicolas Rocha Pacheco <n.nicolas98@hotmail.com> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jaeminSHIN <91681721+woawo1213@users.noreply.github.com> Co-authored-by: Afif Swaidan <53655365+afifswaidan@users.noreply.github.com> Co-authored-by: Afif Swaidan <afif.swaidan@spexal.com>
moss-ag · Mar 4, 2023 · fa50e02 · fa50e02
1 parent fc3eb85
commit fa50e02
Show file tree

Hide file tree

Showing 178 changed files with 8,979 additions and 484 deletions.
diff --git a/.circleci/config.yml b/.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:

diff --git a/Dockerfile b/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

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

diff --git a/nav2_amcl/src/amcl_node.cpp b/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();

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

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp b/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_
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp b/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_
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/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

diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -33,7 +33,7 @@
  <input_port name="server_timeout">Server timeout</input_port>
  </Action>
 
- <Action ID="DriveOnHeadingCancel">
+ <Action ID="CancelDriveOnHeading">
  <input_port name="service_name">Service name to cancel the drive on heading behavior</input_port>
  <input_port name="server_timeout">Server timeout</input_port>
  </Action>
@@ -43,6 +43,11 @@
  <input_port name="server_timeout">Server timeout</input_port>
  </Action>
 
+ <Action ID="CancelAssistedTeleop">
+ <input_port name="service_name">Service name to cancel the assisted teleop behavior</input_port>
+ <input_port name="server_timeout">Server timeout</input_port>
+ </Action>
+
  <Action ID="CancelWait">
  <input_port name="service_name">Service name to cancel the wait behavior</input_port>
  <input_port name="server_timeout">Server timeout</input_port>
@@ -53,6 +58,18 @@
  <input_port name="server_timeout">Server timeout</input_port>
  </Action>
 
+ <Action ID="ClearCostmapExceptRegion">
+ <input_port name="service_name">Service name</input_port>
+ <input_port name="server_timeout">Server timeout</input_port>
+ <input_port name="reset_distance">Distance from the robot above which obstacles are cleared</input_port>
+ </Action>
+
+ <Action ID="ClearCostmapAroundRobot">
+ <input_port name="service_name">Service name</input_port>
+ <input_port name="server_timeout">Server timeout</input_port>
+ <input_port name="reset_distance">Distance from the robot under which obstacles are cleared</input_port>
+ </Action>
+
  <Action ID="ComputePathToPose">
  <input_port name="goal">Destination to plan to</input_port>
  <input_port name="start">Start pose of the path if overriding current robot pose</input_port>
@@ -166,6 +183,13 @@
  <input_port name="server_timeout">Server timeout</input_port>
  </Action>
 
+ <Action ID="AssistedTeleop">
+ <input_port name="time_allowance">Allowed time for spinning</input_port>
+ <input_port name="is_recovery">If true recovery count will be incremented</input_port>
+ <input_port name="server_name">Service name</input_port>
+ <input_port name="server_timeout">Server timeout</input_port>
+ </Action>
+
  <!-- ############################### CONDITION NODES ############################## -->
  <Condition ID="GoalReached">
  <input_port name="goal">Destination</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.2</version>
+ <version>1.1.3</version>
  <description>TODO</description>
  <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
  <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>

diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp
@@ -0,0 +1,62 @@
+// 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.
+
+#include <string>
+#include <memory>
+
+#include "nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+AssistedTeleopAction::AssistedTeleopAction(
+ const std::string & xml_tag_name,
+ const std::string & action_name,
+ const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::AssistedTeleop>(xml_tag_name, action_name, conf)
+{
+ double time_allowance;
+ getInput("time_allowance", time_allowance);
+ getInput("is_recovery", is_recovery_);
+
+ // Populate the input message
+ goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
+}
+
+void AssistedTeleopAction::on_tick()
+{
+ if (is_recovery_) {
+ increment_recovery_count();
+ }
+}
+
+BT::NodeStatus AssistedTeleopAction::on_aborted()
+{
+ return is_recovery_ ? BT::NodeStatus::FAILURE : BT::NodeStatus::SUCCESS;
+}
+
+} // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+ BT::NodeBuilder builder =
+ [](const std::string & name, const BT::NodeConfiguration & config)
+ {
+ return std::make_unique<nav2_behavior_tree::AssistedTeleopAction>(
+ name, "assisted_teleop", config);
+ };
+
+ factory.registerBuilder<nav2_behavior_tree::AssistedTeleopAction>("AssistedTeleop", builder);
+}