-
Notifications
You must be signed in to change notification settings - Fork 1.2k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* remove exec_depend on behaviortree_cpp_v3 (#3279) * BT Service Node to throw if service was not available in time (#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 (#3332) * Remove unused velocity scaling config from example xml (#3330) Signed-off-by: Borong Yuan <yuanborong@hotmail.com> Signed-off-by: Borong Yuan <yuanborong@hotmail.com> * Make mapUpdateLoop() indicator variables to be thread-safe (#3308) * Ensure that plugin initialization to be called before updating routines (#3307) * Solve bug when CostmapInfoServer is reactivated (#3292) * Solve bug when CostmapInfoServer is reactivated * Implemented smoother selector bt node (#3283) * Implemented smoother selector bt node Signed-off-by: Owen Hooper <17ofh@queensu.ca> * updated copyright in modified file Signed-off-by: Owen Hooper <17ofh@queensu.ca> Signed-off-by: Owen Hooper <17ofh@queensu.ca> * Add allow_unknown parameter to theta star planner (#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 <yuanborong@hotmail.com> Signed-off-by: Owen Hooper <17ofh@queensu.ca> Co-authored-by: Adam Aposhian <aposhian.dev@gmail.com> Co-authored-by: Erwin Lejeune <erwin.lejeune15@gmail.com> Co-authored-by: Sven Langner <svenlr@users.noreply.github.com> Co-authored-by: Borong Yuan <yuanborong@hotmail.com> Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Co-authored-by: MartiBolet <43337758+MartiBolet@users.noreply.github.com> Co-authored-by: Owen Hooper <17ofh@queensu.ca> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
- Loading branch information
9 people
committed
Dec 21, 2022
1 parent
0d86469
commit ed7f5ef
Showing
62 changed files
with
765 additions
and
75 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
99 changes: 99 additions & 0 deletions
99
nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
92 changes: 92 additions & 0 deletions
92
nav2_behavior_tree/plugins/action/smoother_selector_node.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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"); | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.