forked from ros-navigation/navigation2
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Drive on heading (ros-navigation#2898)
* renamed class * rename backup to drive_on_heading * test file rename * renamed backup bt node * renamed ports * renamed binaries * named backup cancel * renamed backup action * rename yaml * fixing tests * undid cmake * add backup back in for old users * added back_up back in * remove commented tests * rename server * code review * code review fixes * code review * templated the drive on heading * fully templated drive on heading * template fixes for drive on heading * template working * linting and boolean fix * fixed tests * added multiple files for recoveries * xml fix * fixed copyright * revert * Update nav2_behaviors/plugins/drive_on_heading.hpp * Update nav2_behaviors/plugins/drive_on_heading.hpp * update circleci config * fixed back_up test * fixed drive on heading test Co-authored-by: Steve Macenski <[email protected]>
- Loading branch information
Showing
37 changed files
with
1,600 additions
and
242 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
60 changes: 60 additions & 0 deletions
60
nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.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,60 @@ | ||
| // Copyright (c) 2018 Intel Corporation | ||
| // | ||
| // 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__DRIVE_ON_HEADING_ACTION_HPP_ | ||
| #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_ACTION_HPP_ | ||
|
|
||
| #include <string> | ||
|
|
||
| #include "nav2_behavior_tree/bt_action_node.hpp" | ||
| #include "nav2_msgs/action/drive_on_heading.hpp" | ||
|
|
||
| namespace nav2_behavior_tree | ||
| { | ||
|
|
||
| /** | ||
| * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::DriveOnHeading | ||
| */ | ||
| class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeading> | ||
| { | ||
| public: | ||
| /** | ||
| * @brief A constructor for nav2_behavior_tree::DriveOnHeadingAction | ||
| * @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 | ||
| */ | ||
| DriveOnHeadingAction( | ||
| 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( | ||
| { | ||
| BT::InputPort<double>("dist_to_travel", 0.15, "Distance to travel"), | ||
| BT::InputPort<double>("speed", 0.025, "Speed at which to travel"), | ||
| BT::InputPort<double>("time_allowance", 10.0, "Allowed time for driving on heading") | ||
| }); | ||
| } | ||
| }; | ||
|
|
||
| } // namespace nav2_behavior_tree | ||
|
|
||
| #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_ACTION_HPP_ |
59 changes: 59 additions & 0 deletions
59
..._behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_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,59 @@ | ||
| // Copyright (c) 2022 Neobotix GmbH | ||
| // | ||
| // 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__DRIVE_ON_HEADING_CANCEL_NODE_HPP_ | ||
| #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_CANCEL_NODE_HPP_ | ||
|
|
||
| #include <memory> | ||
| #include <string> | ||
|
|
||
| #include "nav2_msgs/action/drive_on_heading.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::DriveOnHeading | ||
| */ | ||
| class DriveOnHeadingCancel : public BtCancelActionNode<nav2_msgs::action::DriveOnHeading> | ||
| { | ||
| public: | ||
| /** | ||
| * @brief A constructor for nav2_behavior_tree::DriveOnHeadingCancel | ||
| * @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 | ||
| */ | ||
| DriveOnHeadingCancel( | ||
| 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__DRIVE_ON_HEADING_CANCEL_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
57 changes: 57 additions & 0 deletions
57
nav2_behavior_tree/plugins/action/drive_on_heading_action.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,57 @@ | ||
| // Copyright (c) 2018 Intel Corporation | ||
| // | ||
| // 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/drive_on_heading_action.hpp" | ||
|
|
||
| namespace nav2_behavior_tree | ||
| { | ||
|
|
||
| DriveOnHeadingAction::DriveOnHeadingAction( | ||
| const std::string & xml_tag_name, | ||
| const std::string & action_name, | ||
| const BT::NodeConfiguration & conf) | ||
| : BtActionNode<nav2_msgs::action::DriveOnHeading>(xml_tag_name, action_name, conf) | ||
| { | ||
| double dist; | ||
| getInput("dist_to_travel", dist); | ||
| double speed; | ||
| getInput("speed", speed); | ||
| double time_allowance; | ||
| getInput("time_allowance", time_allowance); | ||
|
|
||
| // Populate the input message | ||
| goal_.target.x = dist; | ||
| goal_.target.y = 0.0; | ||
| goal_.target.z = 0.0; | ||
| goal_.speed = speed; | ||
| goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); | ||
| } | ||
|
|
||
| } // 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::DriveOnHeadingAction>( | ||
| name, "drive_on_heading", config); | ||
| }; | ||
|
|
||
| factory.registerBuilder<nav2_behavior_tree::DriveOnHeadingAction>("DriveOnHeading", builder); | ||
| } |
47 changes: 47 additions & 0 deletions
47
nav2_behavior_tree/plugins/action/drive_on_heading_cancel_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,47 @@ | ||
| // Copyright (c) 2022 Neobotix GmbH | ||
| // | ||
| // 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/drive_on_heading_cancel_node.hpp" | ||
|
|
||
| namespace nav2_behavior_tree | ||
| { | ||
|
|
||
| DriveOnHeadingCancel::DriveOnHeadingCancel( | ||
| const std::string & xml_tag_name, | ||
| const std::string & action_name, | ||
| const BT::NodeConfiguration & conf) | ||
| : BtCancelActionNode<nav2_msgs::action::DriveOnHeading>(xml_tag_name, action_name, conf) | ||
| { | ||
| } | ||
|
|
||
| } // 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::DriveOnHeadingCancel>( | ||
| name, "drive_on_heading", config); | ||
| }; | ||
|
|
||
| factory.registerBuilder<nav2_behavior_tree::DriveOnHeadingCancel>( | ||
| "CancelDriveOnHeading", builder); | ||
| } |
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.