Skip to content

Commit

Permalink
Humble sync 2: Nov 8 (ros-navigation#3274)
Browse files Browse the repository at this point in the history
* 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>
  • Loading branch information
17 people authored and shrijitsingh99 committed Mar 4, 2023
1 parent c0e6dfb commit 1a199f9
Show file tree
Hide file tree
Showing 178 changed files with 8,979 additions and 484 deletions.
7 changes: 4 additions & 3 deletions .circleci/config.yml
Original file line number Diff line number Diff line change
Expand Up @@ -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>\
Expand All @@ -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 }}\
Expand Down Expand Up @@ -447,6 +447,7 @@ _environments:
RCUTILS_LOGGING_BUFFERED_STREAM: "0"
RCUTILS_LOGGING_USE_STDOUT: "0"
DEBIAN_FRONTEND: "noninteractive"
PYTHONUNBUFFERED: "1"

executors:
release_exec:
Expand Down
1 change: 1 addition & 0 deletions Dockerfile
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
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.2</version>
<version>1.1.3</version>
<description>
<p>
amcl is a probabilistic localization system for a robot moving in
Expand Down
35 changes: 35 additions & 0 deletions nav2_amcl/src/amcl_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
6 changes: 6 additions & 0 deletions nav2_behavior_tree/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand All @@ -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)

Expand Down
Original file line number Diff line number Diff line change
@@ -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_
Original file line number Diff line number Diff line change
@@ -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_
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
26 changes: 25 additions & 1 deletion nav2_behavior_tree/nav2_tree_nodes.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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>
Expand All @@ -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>
Expand Down Expand Up @@ -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>
Expand Down
2 changes: 1 addition & 1 deletion 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.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>
Expand Down
62 changes: 62 additions & 0 deletions nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp
Original file line number Diff line number Diff line change
@@ -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);
}
Loading

0 comments on commit 1a199f9

Please sign in to comment.