Skip to content

Commit

Permalink
Humble sync 10: April 4, 2024 (#4241)
Browse files Browse the repository at this point in the history
* change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070)

Signed-off-by: GoesM <[email protected]>
Co-authored-by: GoesM <[email protected]>

* Allow path end pose deviation revive (#4065)

* Support stitching paths in compute path to poses

* Update nav2_planner/src/planner_server.cpp

Co-authored-by: Steve Macenski <[email protected]>

* Rename parameter to allow_path_through_poses_goal_deviation

* Fix description

* restore nav2_params

* missing whitespace

* lint fix

* removed parameter

Signed-off-by: gg <[email protected]>

* Update planner_server.hpp

* Update planner_server.cpp

---------

Signed-off-by: gg <[email protected]>
Co-authored-by: pepisg <[email protected]>
Co-authored-by: Pedro Alejandro González <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

* Change costmap_queue to shared library (#4072)

Signed-off-by: cybaol <[email protected]>

* free `map_sub_` before `map_free(map_)` to avoid UAF&&NullPtr bug mentioned in #4078 (#4079)

* free `map_sub_` before `map_free(map_)`

Signed-off-by: GoesM <[email protected]>

* reformat

Signed-off-by: GoesM <[email protected]>

---------

Signed-off-by: GoesM <[email protected]>
Co-authored-by: GoesM <[email protected]>

* adding progress checker selector BT node (#4109)

* New MPPI Cost Critic (Contrib: Brice Renaudeau) (#4090)

* Share code

Signed-off-by: Brice <[email protected]>

* Update inflation_cost_critic.hpp

- copyright
- ifndef

Signed-off-by: Brice <[email protected]>

* fix lint cpp

- extra space

Signed-off-by: Brice <[email protected]>

* Fix Smac Planner confined collision checker  (#4055)

* Update collision_checker.cpp

Signed-off-by: Steve Macenski <[email protected]>

* Fix tests

Signed-off-by: Steve Macenski <[email protected]>

* Update test_a_star.cpp

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: Brice <[email protected]>

* Prevent analytic expansions from shortcutting Smac Planner feasible paths (#3962)

* a potential solution to smac shortcutting

* costmap reoslution

* some fixes

* completed prototype

* some fixes for collision detection and performance

* completing shortcutting fix

* updating tests

* adding readme

---------

Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: Brice <[email protected]>

* change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070)

Signed-off-by: GoesM <[email protected]>
Co-authored-by: GoesM <[email protected]>
Signed-off-by: Brice <[email protected]>

* [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (#4067)

* prototype to test SE2 footprint H improvements

* some fixes

* fixed

* invert logic

* Working final prototype to be tested

* complete unit test conversions

* Update inflation_layer.hpp

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: Brice <[email protected]>

* Adding new Smac paper to readme

Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: Brice <[email protected]>

* Update README.md

Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: Brice <[email protected]>

* [behavior_tree] don't repeat yourself in "blackboard->set" (#4074)

* don't repeat yourself: templates in tests

Signed-off-by: Davide Faconti <[email protected]>

* misse change

Signed-off-by: Davide Faconti <[email protected]>

---------

Signed-off-by: Davide Faconti <[email protected]>
Signed-off-by: Brice <[email protected]>

* Allow path end pose deviation revive (#4065)

* Support stitching paths in compute path to poses

* Update nav2_planner/src/planner_server.cpp

Co-authored-by: Steve Macenski <[email protected]>

* Rename parameter to allow_path_through_poses_goal_deviation

* Fix description

* restore nav2_params

* missing whitespace

* lint fix

* removed parameter

Signed-off-by: gg <[email protected]>

* Update planner_server.hpp

* Update planner_server.cpp

---------

Signed-off-by: gg <[email protected]>
Co-authored-by: pepisg <[email protected]>
Co-authored-by: Pedro Alejandro González <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Brice <[email protected]>

* Updated code to use getInflationLayer() method (#4076)

* updated code to use getInflationLayer method

Signed-off-by: Jose Faria <[email protected]>

* Fix linting

Signed-off-by: Jose Faria <[email protected]>

---------

Signed-off-by: Jose Faria <[email protected]>
Signed-off-by: Brice <[email protected]>

* 1594 twist stamped publisher (#4077)

* Add TwistStamped to controller_server via TwistPublisher util

* Add a new util class for publishing either Twist or TwistStamped
* Add a new parameter for selecting to stamp the twist data
* Consume TwistPublisher in nav2_controller

Signed-off-by: Ryan Friedman <[email protected]>

* Fix small issues

* Unused variable
* Incorrect doxygen

Signed-off-by: Ryan Friedman <[email protected]>

* Remove stored node and assert

Signed-off-by: Ryan Friedman <[email protected]>

* Add tests for node

* Facing timeout even though it does the same thing as velocity smoother test

Signed-off-by: Ryan Friedman <[email protected]>

* Add missing spin call to solve timeout

Signed-off-by: Ryan Friedman <[email protected]>

* Fix copyright (me instead of intel)

Signed-off-by: Ryan Friedman <[email protected]>

* Add full test coverage with subscriber

Signed-off-by: Ryan Friedman <[email protected]>

* Remove unused rclcpp fixture

* Can't use it due to needing to join the pub thread after rclcpp shuts down

Signed-off-by: Ryan Friedman <[email protected]>

* Use TwistStamped in nav2_behaviors

Signed-off-by: Ryan Friedman <[email protected]>

* Use TwistStamped in collision monitor node

Signed-off-by: Ryan Friedman <[email protected]>

* Add TwistStamped readme updates to velocity smoother

Signed-off-by: Ryan Friedman <[email protected]>

* Add TwistSubscriber implementation

Signed-off-by: Ryan Friedman <[email protected]>

* Fix syntax errors

Signed-off-by: Ryan Friedman <[email protected]>

* Use TwistSubscriber in test_velocity_smoother

Signed-off-by: Ryan Friedman <[email protected]>

* Use TwistSubscriber in assisted_teleop

Signed-off-by: Ryan Friedman <[email protected]>

* Use TwistSubscriber in collision monitor node

Signed-off-by: Ryan Friedman <[email protected]>

* Use TwistSubscriber in velocity smoother

Signed-off-by: Ryan Friedman <[email protected]>

* Remove unused code

Signed-off-by: Ryan Friedman <[email protected]>

* add timestamp and frame_id to TwistStamped message

* Add missing utility include

Signed-off-by: Ryan Friedman <[email protected]>

* Document TwistPublisher and TwistSubscriber usage

Signed-off-by: Ryan Friedman <[email protected]>

* Use pass-by-reference

* Instead of std::move(std::unique_ptr<TwistStamped>)

Signed-off-by: Ryan Friedman <[email protected]>

* Finish twist subscriber tests

Signed-off-by: Ryan Friedman <[email protected]>

* Add other constructor and docs

Signed-off-by: Ryan Friedman <[email protected]>

* Fix linter issues

Signed-off-by: Ryan Friedman <[email protected]>

* Manually fix paren alignment

Signed-off-by: Ryan Friedman <[email protected]>

* Remove GSoC reference

Signed-off-by: Ryan Friedman <[email protected]>

* Document twist bool param in README

Signed-off-by: Ryan Friedman <[email protected]>

* Handle twistPublisher in collision monitor

* Implement behavior in the stamped callback
* Unstamped callback calls the stamped callback
* Switch to unique pointer for publisher

Signed-off-by: Ryan Friedman <[email protected]>

* Convert to using TwistStamped interally

* Use incoming twistStamped timestamp if available
* Convert all internal representations to use TwistStamped

Signed-off-by: Ryan Friedman <[email protected]>

* Remove nav2_util usage instructions

Signed-off-by: Ryan Friedman <[email protected]>

* Remove unused Twist only subscriber

Signed-off-by: Ryan Friedman <[email protected]>

* More linter fixes

Signed-off-by: Ryan Friedman <[email protected]>

* Prefer working with unique_ptr for cmd_vel

* This makes it easier to switch to std::move instead of dereference on
  publish

Signed-off-by: Ryan Friedman <[email protected]>

* Completing twist stamped migration

* shared to unique ptr

Signed-off-by: Steve Macenski <[email protected]>

* twist add stamps and properly propogated

* nav2_util: fix for compiling with clang

- Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move]

Signed-off-by: Rhys Mainwaring <[email protected]>

---------

Signed-off-by: Ryan Friedman <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: Rhys Mainwaring <[email protected]>
Co-authored-by: pedro-fuoco <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>
Co-authored-by: Rhys Mainwaring <[email protected]>
Signed-off-by: Brice <[email protected]>

* Change costmap_queue to shared library (#4072)

Signed-off-by: cybaol <[email protected]>
Signed-off-by: Brice <[email protected]>

* fix include of hpp

Signed-off-by: Brice Renaudeau <[email protected]>

* inflation cost optmiizations and cleanu

* rename, add defaults, and docs

* smoke test addition

* lintg

* normalize weight

* update readme

* increment cache

* Update cost_critic.hpp

Signed-off-by: Steve Macenski <[email protected]>

* Update cost_critic.cpp

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Brice <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: GoesM <[email protected]>
Signed-off-by: Davide Faconti <[email protected]>
Signed-off-by: gg <[email protected]>
Signed-off-by: Jose Faria <[email protected]>
Signed-off-by: Ryan Friedman <[email protected]>
Signed-off-by: Rhys Mainwaring <[email protected]>
Signed-off-by: cybaol <[email protected]>
Signed-off-by: Brice Renaudeau <[email protected]>
Co-authored-by: BriceRenaudeau <[email protected]>
Co-authored-by: GoesM <[email protected]>
Co-authored-by: GoesM <[email protected]>
Co-authored-by: Davide Faconti <[email protected]>
Co-authored-by: Joshua Wallace <[email protected]>
Co-authored-by: pepisg <[email protected]>
Co-authored-by: Pedro Alejandro González <[email protected]>
Co-authored-by: jncfa <[email protected]>
Co-authored-by: Ryan <[email protected]>
Co-authored-by: pedro-fuoco <[email protected]>
Co-authored-by: Rhys Mainwaring <[email protected]>
Co-authored-by: Kino <[email protected]>

* Use ament_export_targets for all targets (#4112)

* Matches new internal ALIAS targets
* Use ALIAS targets for all internal linkage
* Remove unnecessary calls to ament_target_dependencies in test code
* Export includes in proper folders for overlays in colcon

Signed-off-by: Ryan Friedman <[email protected]>

* Update default recommendation from Obstacles to Cost critic in MPPI (#4170)

Signed-off-by: Steve Macenski <[email protected]>

* completely shutdown inital_pose_sub_ (#4176)

Signed-off-by: GoesM <[email protected]>
Co-authored-by: GoesM <[email protected]>

* fix typos in description messages (#4188)

Signed-off-by: Antonio Park <[email protected]>

* AMCL: Set an initial guess by service call (#4182)

* Added initial guess service. Signed-off-by: Alexander Mock

Signed-off-by: Alexander Mock <[email protected]>

* - Removed added empty line
- Renamed initialGuessCallback to initialPoseReceivedSrv
- Added new line to SetInitialPose service definition
- Removed mutex from initialPoseReceived
- Cleanup service server

Signed-off-by: Alexander Mock <[email protected]>

* added whitespace

Signed-off-by: Alexander Mock <[email protected]>

* renamed initial pose service in callback bind

Signed-off-by: Alexander Mock <[email protected]>

---------

Signed-off-by: Alexander Mock <[email protected]>

* Move lines for pre-computation to outside a loop (#4191)

Signed-off-by: Kyungsik Park <[email protected]>

* Fix typo (#4196)

* Fix BT.CPP import

Signed-off-by: Tony Najjar <[email protected]>

* Update README.md

---------

Signed-off-by: Tony Najjar <[email protected]>

* Update footprint iif changed (#4193)

Signed-off-by: Brice <[email protected]>

* bump humble to 1.1.14 for sync

* fix merge conflict

* custom version of cost critic's cost method

---------

Signed-off-by: GoesM <[email protected]>
Signed-off-by: gg <[email protected]>
Signed-off-by: cybaol <[email protected]>
Signed-off-by: Brice <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: Davide Faconti <[email protected]>
Signed-off-by: Jose Faria <[email protected]>
Signed-off-by: Ryan Friedman <[email protected]>
Signed-off-by: Rhys Mainwaring <[email protected]>
Signed-off-by: Brice Renaudeau <[email protected]>
Signed-off-by: Antonio Park <[email protected]>
Signed-off-by: Alexander Mock <[email protected]>
Signed-off-by: Kyungsik Park <[email protected]>
Signed-off-by: Tony Najjar <[email protected]>
Co-authored-by: GoesM <[email protected]>
Co-authored-by: GoesM <[email protected]>
Co-authored-by: Joshua Wallace <[email protected]>
Co-authored-by: pepisg <[email protected]>
Co-authored-by: Pedro Alejandro González <[email protected]>
Co-authored-by: Kino <[email protected]>
Co-authored-by: BriceRenaudeau <[email protected]>
Co-authored-by: Davide Faconti <[email protected]>
Co-authored-by: jncfa <[email protected]>
Co-authored-by: Ryan <[email protected]>
Co-authored-by: pedro-fuoco <[email protected]>
Co-authored-by: Rhys Mainwaring <[email protected]>
Co-authored-by: Antonio Park <[email protected]>
Co-authored-by: Alexander Mock <[email protected]>
Co-authored-by: Tony Najjar <[email protected]>
  • Loading branch information
16 people authored Apr 4, 2024
1 parent 9d9db1b commit f41920e
Show file tree
Hide file tree
Showing 66 changed files with 886 additions and 138 deletions.
11 changes: 11 additions & 0 deletions nav2_amcl/include/nav2_amcl/amcl_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
#include "nav2_amcl/sensors/laser/laser.hpp"
#include "nav2_msgs/msg/particle.hpp"
#include "nav2_msgs/msg/particle_cloud.hpp"
#include "nav2_msgs/srv/set_initial_pose.hpp"
#include "nav_msgs/srv/set_map.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "std_srvs/srv/empty.hpp"
Expand Down Expand Up @@ -210,6 +211,16 @@ class AmclNode : public nav2_util::LifecycleNode
const std::shared_ptr<std_srvs::srv::Empty::Request> request,
std::shared_ptr<std_srvs::srv::Empty::Response> response);

// service server for providing an initial pose guess
rclcpp::Service<nav2_msgs::srv::SetInitialPose>::SharedPtr initial_guess_srv_;
/*
* @brief Service callback for an initial pose guess request
*/
void initialPoseReceivedSrv(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<nav2_msgs::srv::SetInitialPose::Request> request,
std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response> response);

// Let amcl update samples without requiring motion
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr nomotion_update_srv_;
/*
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.13</version>
<version>1.1.14</version>
<description>
<p>
amcl is a probabilistic localization system for a robot moving in
Expand Down
26 changes: 21 additions & 5 deletions nav2_amcl/src/amcl_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -325,13 +325,17 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
// Get rid of the inputs first (services and message filter input), so we
// don't continue to process incoming messages
global_loc_srv_.reset();
initial_guess_srv_.reset();
nomotion_update_srv_.reset();
executor_thread_.reset(); // to make sure initial_pose_sub_ completely exit
initial_pose_sub_.reset();
laser_scan_connection_.disconnect();
tf_listener_.reset(); // listener may access lase_scan_filter_, so it should be reset earlier
laser_scan_filter_.reset();
laser_scan_sub_.reset();

// Map
map_sub_.reset(); // map_sub_ may access map_, so it should be reset earlier
if (map_ != NULL) {
map_free(map_);
map_ = nullptr;
Expand All @@ -341,7 +345,6 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)

// Transforms
tf_broadcaster_.reset();
tf_listener_.reset();
tf_buffer_.reset();

// PubSub
Expand Down Expand Up @@ -493,6 +496,15 @@ AmclNode::globalLocalizationCallback(
pf_init_ = false;
}

void
AmclNode::initialPoseReceivedSrv(
const std::shared_ptr<rmw_request_id_t>/*request_header*/,
const std::shared_ptr<nav2_msgs::srv::SetInitialPose::Request> req,
std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response>/*res*/)
{
initialPoseReceived(std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(req->pose));
}

// force nomotion updates (amcl updating without requiring motion)
void
AmclNode::nomotionUpdateCallback(
Expand Down Expand Up @@ -1093,20 +1105,20 @@ AmclNode::initParameters()
// Semantic checks
if (laser_likelihood_max_dist_ < 0) {
RCLCPP_WARN(
get_logger(), "You've set laser_likelihood_max_dist to be negtive,"
get_logger(), "You've set laser_likelihood_max_dist to be negative,"
" this isn't allowed so it will be set to default value 2.0.");
laser_likelihood_max_dist_ = 2.0;
}
if (max_particles_ < 0) {
RCLCPP_WARN(
get_logger(), "You've set max_particles to be negtive,"
get_logger(), "You've set max_particles to be negative,"
" this isn't allowed so it will be set to default value 2000.");
max_particles_ = 2000;
}

if (min_particles_ < 0) {
RCLCPP_WARN(
get_logger(), "You've set min_particles to be negtive,"
get_logger(), "You've set min_particles to be negative,"
" this isn't allowed so it will be set to default value 500.");
min_particles_ = 500;
}
Expand All @@ -1120,7 +1132,7 @@ AmclNode::initParameters()

if (resample_interval_ <= 0) {
RCLCPP_WARN(
get_logger(), "You've set resample_interval to be zero or negtive,"
get_logger(), "You've set resample_interval to be zero or negative,"
" this isn't allowed so it will be set to default value to 1.");
resample_interval_ = 1;
}
Expand Down Expand Up @@ -1533,6 +1545,10 @@ AmclNode::initServices()
"reinitialize_global_localization",
std::bind(&AmclNode::globalLocalizationCallback, this, _1, _2, _3));

initial_guess_srv_ = create_service<nav2_msgs::srv::SetInitialPose>(
"set_initial_pose",
std::bind(&AmclNode::initialPoseReceivedSrv, this, _1, _2, _3));

nomotion_update_srv_ = create_service<std_srvs::srv::Empty>(
"request_nomotion_update",
std::bind(&AmclNode::nomotionUpdateCallback, this, _1, _2, _3));
Expand Down
22 changes: 11 additions & 11 deletions nav2_amcl/src/sensors/laser/likelihood_field_model.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,17 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set)

self = reinterpret_cast<LikelihoodFieldModel *>(data->laser);

// Pre-compute a couple of things
double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_;
double z_rand_mult = 1.0 / data->range_max;

step = (data->range_count - 1) / (self->max_beams_ - 1);

// Step size must be at least 1
if (step < 1) {
step = 1;
}

total_weight = 0.0;

// Compute the sample weights
Expand All @@ -65,17 +76,6 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set)

p = 1.0;

// Pre-compute a couple of things
double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_;
double z_rand_mult = 1.0 / data->range_max;

step = (data->range_count - 1) / (self->max_beams_ - 1);

// Step size must be at least 1
if (step < 1) {
step = 1;
}

for (i = 0; i < data->range_count; i += step) {
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
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 @@ -192,6 +192,9 @@ 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)

add_library(nav2_progress_checker_selector_bt_node SHARED plugins/action/progress_checker_selector_node.cpp)
list(APPEND plugin_libs nav2_progress_checker_selector_bt_node)

add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp)
list(APPEND plugin_libs nav2_goal_updated_controller_bt_node)

Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,96 @@
// Copyright (c) 2024 Open Navigation LLC
//
// 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__PROGRESS_CHECKER_SELECTOR_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_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 ProgressCheckerSelector behavior is used to switch the progress checker
* of the controller server. It subscribes to a topic "progress_checker_selector"
* to get the decision about what progress_checker must be used. It is usually used before of
* the FollowPath. The selected_progress_checker output port is passed to progress_checker_id
* input port of the FollowPath
*/
class ProgressCheckerSelector : public BT::SyncActionNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::ProgressCheckerSelector
*
* @param xml_tag_name Name for the XML tag for this node
* @param conf BT node configuration
*/
ProgressCheckerSelector(
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_progress_checker",
"the default progress_checker to use if there is not any external topic message received."),

BT::InputPort<std::string>(
"topic_name",
"progress_checker_selector",
"the input topic name to select the progress_checker"),

BT::OutputPort<std::string>(
"selected_progress_checker",
"Selected progress_checker by subscription")
};
}

private:
/**
* @brief Function to perform some user-defined operation on tick
*/
BT::NodeStatus tick() override;

/**
* @brief callback function for the progress_checker_selector topic
*
* @param msg the message with the id of the progress_checker_selector
*/
void callbackProgressCheckerSelect(const std_msgs::msg::String::SharedPtr msg);

rclcpp::Subscription<std_msgs::msg::String>::SharedPtr progress_checker_selector_sub_;

std::string last_selected_progress_checker_;

rclcpp::Node::SharedPtr node_;

std::string topic_name_;
};

} // namespace nav2_behavior_tree

#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_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 @@ -174,6 +174,12 @@
<output_port name="selected_goal_checker">Name of the selected goal checker received from the topic subcription</output_port>
</Action>

<Action ID="ProgressCheckerSelector">
<input_port name="topic_name">Name of the topic to receive progress checker selection commands</input_port>
<input_port name="default_progress_checker">Default progress checker of the controller selector</input_port>
<output_port name="selected_progress_checker">Name of the selected progress checker received from the topic subcription</output_port>
</Action>

<Action ID="Spin">
<input_port name="spin_dist">Spin distance</input_port>
<input_port name="time_allowance">Allowed time for spinning</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.13</version>
<version>1.1.14</version>
<description>TODO</description>
<maintainer email="[email protected]">Michael Jeronimo</maintainer>
<maintainer email="[email protected]">Carlos Orduno</maintainer>
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,81 @@
// Copyright (c) 2024 Open Navigation LLC
//
// 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/progress_checker_selector_node.hpp"

#include "rclcpp/rclcpp.hpp"

namespace nav2_behavior_tree
{

using std::placeholders::_1;

ProgressCheckerSelector::ProgressCheckerSelector(
const std::string & name,
const BT::NodeConfiguration & conf)
: BT::SyncActionNode(name, conf)
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");

getInput("topic_name", topic_name_);

rclcpp::QoS qos(rclcpp::KeepLast(1));
qos.transient_local().reliable();

progress_checker_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
topic_name_, qos, std::bind(&ProgressCheckerSelector::callbackProgressCheckerSelect, this, _1));
}

BT::NodeStatus ProgressCheckerSelector::tick()
{
rclcpp::spin_some(node_);

// This behavior always use the last selected progress checker received from the topic input.
// When no input is specified it uses the default goaprogressl checker.
// If the default progress checker is not specified then we work in
// "required progress checker mode": In this mode, the behavior returns failure if the progress
// checker selection is not received from the topic input.
if (last_selected_progress_checker_.empty()) {
std::string default_progress_checker;
getInput("default_progress_checker", default_progress_checker);
if (default_progress_checker.empty()) {
return BT::NodeStatus::FAILURE;
} else {
last_selected_progress_checker_ = default_progress_checker;
}
}

setOutput("selected_progress_checker", last_selected_progress_checker_);

return BT::NodeStatus::SUCCESS;
}

void
ProgressCheckerSelector::callbackProgressCheckerSelect(const std_msgs::msg::String::SharedPtr msg)
{
last_selected_progress_checker_ = msg->data;
}

} // namespace nav2_behavior_tree

#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<nav2_behavior_tree::ProgressCheckerSelector>("ProgressCheckerSelector");
}
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 @@ -107,3 +107,7 @@ 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})

ament_add_gtest(test_progress_checker_selector_node test_progress_checker_selector_node.cpp)
target_link_libraries(test_progress_checker_selector_node nav2_progress_checker_selector_bt_node)
ament_target_dependencies(test_progress_checker_selector_node ${dependencies})
Loading

0 comments on commit f41920e

Please sign in to comment.