Skip to content

Commit 69fdfd6

Browse files
fmricoSteveMacenski
fmrico
and
SteveMacenski
authored
Follow pose (#1859)
* Truncate path finished Signed-off-by: Francisco Martin Rico <[email protected]> * Follow Pose finished Signed-off-by: Francisco Martin Rico <[email protected]> * Change names. Add test and doc Signed-off-by: Francisco Martin Rico <[email protected]> * Change names and check atan2 values Signed-off-by: Francisco Martin Rico <[email protected]> * Checking Inf/NaNs and trucate path changes Signed-off-by: Francisco Martin Rico <[email protected]> * Revert changes in launcher Signed-off-by: Francisco Martin Rico <[email protected]> * Documenting Tree Signed-off-by: Francisco Martin Rico <[email protected]> * Update nav2_bt_navigator/README.md Co-authored-by: Steve Macenski <[email protected]> * Update nav2_bt_navigator/README.md Co-authored-by: Steve Macenski <[email protected]> * Change TruncatePath from decorator to action node Signed-off-by: Francisco Martin Rico <[email protected]> * Update nav2_bt_navigator/README.md Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Steve Macenski <[email protected]>
1 parent d7494ec commit 69fdfd6

File tree

15 files changed

+651
-2
lines changed

15 files changed

+651
-2
lines changed

‎doc/parameters/param_list.md‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -637,6 +637,14 @@ When `recovery_plugins` parameter is not overridden, the following default plugi
637637
| server_name | N/A | Action server name |
638638
| server_timeout | 10 | Action server timeout (ms) |
639639

640+
### BT Node TruncatePath
641+
642+
| Input Port | Default | Description |
643+
| ---------- | ------- | ----------- |
644+
| input_path | N/A | Path to be truncated |
645+
| output_path | N/A | Path truncated |
646+
| distance | 1.0 | Distance (m) to cut from last pose |
647+
640648
## Conditions
641649

642650
### BT Node GoalReached
@@ -687,3 +695,10 @@ When `recovery_plugins` parameter is not overridden, the following default plugi
687695
| min_speed | 0.0 | Minimum speed (m/s) |
688696
| max_speed | 0.5 | Maximum speed (m/s) |
689697
| filter_duration | 0.3 | Duration (secs) for velocity smoothing filter |
698+
699+
### BT Node GoalUpdater
700+
701+
| Input Port | Default | Description |
702+
| ---------- | ------- | ----------- |
703+
| input_goal | N/A | The reference goal |
704+
| output_goal | N/A | The reference goal, or a newer goal received by subscription |

‎nav2_behavior_tree/CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -101,6 +101,12 @@ list(APPEND plugin_libs nav2_distance_controller_bt_node)
101101
add_library(nav2_speed_controller_bt_node SHARED plugins/decorator/speed_controller.cpp)
102102
list(APPEND plugin_libs nav2_speed_controller_bt_node)
103103

104+
add_library(nav2_truncate_path_action_bt_node SHARED plugins/action/truncate_path_action.cpp)
105+
list(APPEND plugin_libs nav2_truncate_path_action_bt_node)
106+
107+
add_library(nav2_goal_updater_node_bt_node SHARED plugins/decorator/goal_updater_node.cpp)
108+
list(APPEND plugin_libs nav2_goal_updater_node_bt_node)
109+
104110
add_library(nav2_recovery_node_bt_node SHARED plugins/control/recovery_node.cpp)
105111
list(APPEND plugin_libs nav2_recovery_node_bt_node)
106112

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp‎

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,55 @@
1+
// Copyright (c) 2018 Intel Corporation
2+
// Copyright (c) 2020 Francisco Martin Rico
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_
17+
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_
18+
19+
#include <memory>
20+
#include <string>
21+
22+
#include "nav_msgs/msg/path.hpp"
23+
24+
#include "behaviortree_cpp_v3/action_node.h"
25+
26+
namespace nav2_behavior_tree
27+
{
28+
29+
class TruncatePath : public BT::ActionNodeBase
30+
{
31+
public:
32+
TruncatePath(
33+
const std::string & xml_tag_name,
34+
const BT::NodeConfiguration & conf);
35+
36+
37+
static BT::PortsList providedPorts()
38+
{
39+
return {
40+
BT::InputPort<nav_msgs::msg::Path>("input_path", "Original Path"),
41+
BT::OutputPort<nav_msgs::msg::Path>("output_path", "Path truncated to a certain distance"),
42+
BT::InputPort<double>("distance", 1.0, "distance"),
43+
};
44+
}
45+
46+
private:
47+
void halt() override {}
48+
BT::NodeStatus tick() override;
49+
50+
double distance_;
51+
};
52+
53+
} // namespace nav2_behavior_tree
54+
55+
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp‎

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
// Copyright (c) 2018 Intel Corporation
2+
// Copyright (c) 2020 Francisco Martin Rico
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_
17+
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_
18+
19+
#include <memory>
20+
#include <string>
21+
22+
#include "geometry_msgs/msg/pose_stamped.hpp"
23+
24+
#include "behaviortree_cpp_v3/decorator_node.h"
25+
26+
#include "rclcpp/rclcpp.hpp"
27+
28+
namespace nav2_behavior_tree
29+
{
30+
31+
class GoalUpdater : public BT::DecoratorNode
32+
{
33+
public:
34+
GoalUpdater(
35+
const std::string & xml_tag_name,
36+
const BT::NodeConfiguration & conf);
37+
38+
39+
static BT::PortsList providedPorts()
40+
{
41+
return {
42+
BT::InputPort<geometry_msgs::msg::PoseStamped>("input_goal", "Original Goal"),
43+
BT::OutputPort<geometry_msgs::msg::PoseStamped>(
44+
"output_goal",
45+
"Received Goal by subscription"),
46+
};
47+
}
48+
49+
private:
50+
BT::NodeStatus tick() override;
51+
52+
void callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
53+
54+
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
55+
56+
geometry_msgs::msg::PoseStamped last_goal_received_;
57+
};
58+
59+
} // namespace nav2_behavior_tree
60+
61+
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_

‎nav2_behavior_tree/plugins/action/truncate_path_action.cpp‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
1+
// Copyright (c) 2018 Intel Corporation
2+
// Copyright (c) 2020 Francisco Martin Rico
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#include <string>
17+
#include <memory>
18+
#include <limits>
19+
20+
#include "nav_msgs/msg/path.hpp"
21+
#include "geometry_msgs/msg/pose_stamped.hpp"
22+
#include "nav2_util/geometry_utils.hpp"
23+
#include "behaviortree_cpp_v3/decorator_node.h"
24+
25+
#include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp"
26+
27+
namespace nav2_behavior_tree
28+
{
29+
30+
TruncatePath::TruncatePath(
31+
const std::string & name,
32+
const BT::NodeConfiguration & conf)
33+
: BT::ActionNodeBase(name, conf),
34+
distance_(1.0)
35+
{
36+
getInput("distance", distance_);
37+
}
38+
39+
inline BT::NodeStatus TruncatePath::tick()
40+
{
41+
setStatus(BT::NodeStatus::RUNNING);
42+
43+
nav_msgs::msg::Path input_path;
44+
45+
getInput("input_path", input_path);
46+
47+
if (input_path.poses.empty()) {
48+
setOutput("output_path", input_path);
49+
return BT::NodeStatus::SUCCESS;
50+
}
51+
52+
geometry_msgs::msg::PoseStamped final_pose = input_path.poses.back();
53+
54+
double distance_to_goal = nav2_util::geometry_utils::euclidean_distance(
55+
input_path.poses.back(), final_pose);
56+
57+
while (distance_to_goal < distance_ && input_path.poses.size() > 2) {
58+
input_path.poses.pop_back();
59+
distance_to_goal = nav2_util::geometry_utils::euclidean_distance(
60+
input_path.poses.back(), final_pose);
61+
}
62+
63+
double dx = final_pose.pose.position.x - input_path.poses.back().pose.position.x;
64+
double dy = final_pose.pose.position.y - input_path.poses.back().pose.position.y;
65+
66+
double final_angle = atan2(dy, dx);
67+
68+
if (std::isnan(final_angle) || std::isinf(final_angle)) {
69+
RCLCPP_WARN(
70+
config().blackboard->get<rclcpp::Node::SharedPtr>("node")->get_logger(),
71+
"Final angle is not valid while truncating path. Setting to 0.0");
72+
final_angle = 0.0;
73+
}
74+
75+
input_path.poses.back().pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
76+
final_angle);
77+
78+
setOutput("output_path", input_path);
79+
80+
return BT::NodeStatus::SUCCESS;
81+
}
82+
83+
} // namespace nav2_behavior_tree
84+
85+
#include "behaviortree_cpp_v3/bt_factory.h"
86+
BT_REGISTER_NODES(factory)
87+
{
88+
factory.registerNodeType<nav2_behavior_tree::TruncatePath>("TruncatePath");
89+
}

‎nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp‎

Lines changed: 71 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,71 @@
1+
// Copyright (c) 2018 Intel Corporation
2+
// Copyright (c) 2020 Francisco Martin Rico
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#include <string>
17+
#include <memory>
18+
19+
#include "geometry_msgs/msg/pose_stamped.hpp"
20+
#include "behaviortree_cpp_v3/decorator_node.h"
21+
22+
#include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp"
23+
24+
#include "rclcpp/rclcpp.hpp"
25+
26+
namespace nav2_behavior_tree
27+
{
28+
29+
using std::placeholders::_1;
30+
31+
GoalUpdater::GoalUpdater(
32+
const std::string & name,
33+
const BT::NodeConfiguration & conf)
34+
: BT::DecoratorNode(name, conf)
35+
{
36+
auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
37+
38+
std::string goal_updater_topic;
39+
node->get_parameter_or<std::string>("goal_updater_topic", goal_updater_topic, "goal_update");
40+
41+
goal_sub_ = node->create_subscription<geometry_msgs::msg::PoseStamped>(
42+
goal_updater_topic, 10, std::bind(&GoalUpdater::callback_updated_goal, this, _1));
43+
}
44+
45+
inline BT::NodeStatus GoalUpdater::tick()
46+
{
47+
geometry_msgs::msg::PoseStamped goal;
48+
49+
getInput("input_goal", goal);
50+
51+
if (rclcpp::Time(last_goal_received_.header.stamp) > rclcpp::Time(goal.header.stamp)) {
52+
goal = last_goal_received_;
53+
}
54+
55+
setOutput("output_goal", goal);
56+
return child_node_->executeTick();
57+
}
58+
59+
void
60+
GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
61+
{
62+
last_goal_received_ = *msg;
63+
}
64+
65+
} // namespace nav2_behavior_tree
66+
67+
#include "behaviortree_cpp_v3/bt_factory.h"
68+
BT_REGISTER_NODES(factory)
69+
{
70+
factory.registerNodeType<nav2_behavior_tree::GoalUpdater>("GoalUpdater");
71+
}

‎nav2_behavior_tree/test/plugins/action/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,3 +29,7 @@ ament_target_dependencies(test_action_follow_path_action ${dependencies})
2929
ament_add_gtest(test_action_navigate_to_pose_action test_navigate_to_pose_action.cpp)
3030
target_link_libraries(test_action_navigate_to_pose_action nav2_navigate_to_pose_action_bt_node)
3131
ament_target_dependencies(test_action_navigate_to_pose_action ${dependencies})
32+
33+
ament_add_gtest(test_truncate_path_action test_truncate_path_action.cpp)
34+
target_link_libraries(test_truncate_path_action nav2_truncate_path_action_bt_node)
35+
ament_target_dependencies(test_truncate_path_action ${dependencies})

0 commit comments

Comments
 (0)