Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Drive on heading #2898

Merged
merged 36 commits into from
May 4, 2022
Merged
Show file tree
Hide file tree
Changes from 31 commits
Commits
Show all changes
36 commits
Select commit Hold shift + click to select a range
0f25db7
renamed class
jwallace42 Apr 6, 2022
f3d1df8
rename backup to drive_on_heading
jwallace42 Apr 6, 2022
1857b22
test file rename
jwallace42 Apr 6, 2022
ccc952a
renamed backup bt node
jwallace42 Apr 7, 2022
8f4aaff
renamed ports
jwallace42 Apr 7, 2022
21e6665
renamed binaries
jwallace42 Apr 7, 2022
34b4ecc
named backup cancel
jwallace42 Apr 7, 2022
3bf6f42
renamed backup action
jwallace42 Apr 7, 2022
a32ebfa
rename yaml
jwallace42 Apr 7, 2022
e605a99
fixing tests
jwallace42 Apr 8, 2022
0d41190
undid cmake
jwallace42 Apr 8, 2022
067f9c2
add backup back in for old users
jwallace42 Apr 12, 2022
e64a23c
Merge branch 'main' into drive_on_heading
jwallace42 Apr 13, 2022
2d7c418
added back_up back in
jwallace42 Apr 13, 2022
d43df2c
remove commented tests
jwallace42 Apr 13, 2022
9ddcd63
rename server
jwallace42 Apr 13, 2022
bcdcf05
code review
jwallace42 Apr 13, 2022
8566a76
code review fixes
jwallace42 Apr 13, 2022
e2c0ad0
Merge branch 'main' into drive_on_heading
jwallace42 Apr 13, 2022
5bd6234
code review
jwallace42 Apr 13, 2022
3276b9d
templated the drive on heading
jwallace42 Apr 13, 2022
4db84db
fully templated drive on heading
jwallace42 Apr 14, 2022
245a6f4
template fixes for drive on heading
jwallace42 Apr 15, 2022
23c2408
template working
jwallace42 Apr 15, 2022
6604c7b
linting and boolean fix
jwallace42 Apr 18, 2022
b116e91
fixed tests
jwallace42 Apr 18, 2022
bb09abb
added multiple files for recoveries
jwallace42 Apr 19, 2022
aa58be2
xml fix
jwallace42 Apr 19, 2022
2dcca64
fixed copyright
jwallace42 Apr 19, 2022
c04e9f7
revert
jwallace42 Apr 28, 2022
08d721a
Update nav2_behaviors/plugins/drive_on_heading.hpp
SteveMacenski Apr 28, 2022
8195300
Update nav2_behaviors/plugins/drive_on_heading.hpp
SteveMacenski Apr 28, 2022
096727b
Merge branch 'main' into drive_on_heading
jwallace42 Apr 28, 2022
8f72271
update circleci config
jwallace42 Apr 28, 2022
fa06797
fixed back_up test
jwallace42 May 3, 2022
41ff070
fixed drive on heading test
jwallace42 May 3, 2022
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
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_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)

add_library(nav2_smooth_path_action_bt_node SHARED plugins/action/smooth_path_action.cpp)
list(APPEND plugin_libs nav2_smooth_path_action_bt_node)

Expand All @@ -78,6 +81,9 @@ list(APPEND plugin_libs nav2_follow_path_action_bt_node)
add_library(nav2_back_up_action_bt_node SHARED plugins/action/back_up_action.cpp)
list(APPEND plugin_libs nav2_back_up_action_bt_node)

add_library(nav2_drive_on_heading_bt_node SHARED plugins/action/drive_on_heading_action.cpp)
list(APPEND plugin_libs nav2_drive_on_heading_bt_node)

add_library(nav2_spin_action_bt_node SHARED plugins/action/spin_action.cpp)
list(APPEND plugin_libs nav2_spin_action_bt_node)

Expand Down
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_
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_
19 changes: 16 additions & 3 deletions nav2_behavior_tree/nav2_tree_nodes.xml
Original file line number Diff line number Diff line change
Expand Up @@ -15,23 +15,36 @@
<input_port name="server_timeout">Server timeout</input_port>
</Action>

<Action ID="DriveOnHeading">
<input_port name="dist_to_travel">Distance to travel</input_port>
<input_port name="speed">Speed at which to travel</input_port>
<input_port name="time_allowance">Allowed time for reversing</input_port>
<input_port name="server_name">Service name</input_port>
<input_port name="server_timeout">Server timeout</input_port>
</Action>

<Action ID="CancelControl">
<input_port name="service_name">Service name to cancel the controller server</input_port>
<input_port name="server_timeout">Server timeout</input_port>
</Action>

<Action ID="CancelBackUp">
<input_port name="service_name">Service name to cancel the backup recovery</input_port>
<input_port name="service_name">Service name to cancel the backup behavior</input_port>
<input_port name="server_timeout">Server timeout</input_port>
</Action>

<Action ID="DriveOnHeadingCancel">
<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>

<Action ID="CancelSpin">
<input_port name="service_name">Service name to cancel the spin recovery</input_port>
<input_port name="service_name">Service name to cancel the spin 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 recovery</input_port>
<input_port name="service_name">Service name to cancel the wait behavior</input_port>
<input_port name="server_timeout">Server timeout</input_port>
</Action>

Expand Down
57 changes: 57 additions & 0 deletions nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp
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 nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp
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);
}
9 changes: 9 additions & 0 deletions nav2_behavior_tree/test/plugins/action/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,10 @@ ament_add_gtest(test_action_back_up_action test_back_up_action.cpp)
target_link_libraries(test_action_back_up_action nav2_back_up_action_bt_node)
ament_target_dependencies(test_action_back_up_action ${dependencies})

ament_add_gtest(test_action_drive_on_heading test_drive_on_heading_action.cpp)
target_link_libraries(test_action_drive_on_heading nav2_drive_on_heading_bt_node)
ament_target_dependencies(test_action_drive_on_heading ${dependencies})

ament_add_gtest(test_action_wait_action test_wait_action.cpp)
target_link_libraries(test_action_wait_action nav2_wait_action_bt_node)
ament_target_dependencies(test_action_wait_action ${dependencies})
Expand All @@ -27,10 +31,15 @@ ament_add_gtest(test_action_spin_cancel_action test_spin_cancel_node.cpp)
target_link_libraries(test_action_spin_cancel_action nav2_spin_cancel_bt_node)
ament_target_dependencies(test_action_spin_cancel_action ${dependencies})


ament_add_gtest(test_action_back_up_cancel_action test_back_up_cancel_node.cpp)
target_link_libraries(test_action_back_up_cancel_action nav2_back_up_cancel_bt_node)
ament_target_dependencies(test_action_back_up_cancel_action ${dependencies})

ament_add_gtest(test_action_drive_on_heading_cancel_action test_drive_on_heading_cancel_node.cpp)
target_link_libraries(test_action_drive_on_heading_cancel_action nav2_drive_on_heading_cancel_bt_node)
ament_target_dependencies(test_action_drive_on_heading_cancel_action ${dependencies})

ament_add_gtest(test_action_clear_costmap_service test_clear_costmap_service.cpp)
target_link_libraries(test_action_clear_costmap_service nav2_clear_costmap_service_bt_node)
ament_target_dependencies(test_action_clear_costmap_service ${dependencies})
Expand Down
Loading