Follow pose

doc/parameters/param_list.md

@@ -624,6 +624,14 @@ When `recovery_plugins` parameter is not overridden, the following default plugi
 | server_name | N/A | Action server name |
 | server_timeout | 10 | Action server timeout (ms) |
 
+### BT Node TruncatePath
+
+| Input Port | Default | Description |
+| ---------- | ------- | ----------- |
+| input_path | N/A | Path to be truncated |
+| output_path | N/A | Path truncated |
+| distance | 1.0 | Distance (m) to cut from last pose |
+
 ## Conditions
 
 ### BT Node GoalReached
@@ -674,3 +682,10 @@ When `recovery_plugins` parameter is not overridden, the following default plugi
 | min_speed | 0.0 | Minimum speed (m/s) |
 | max_speed | 0.5 | Maximum speed (m/s) |
 | filter_duration | 0.3 | Duration (secs) for velocity smoothing filter |
+
+### BT Node GoalUpdater
+
+| Input Port | Default | Description |
+| ---------- | ------- | ----------- |
+| input_goal | N/A | The reference goal |
+| output_goal | N/A | The reference goal, or a newer goal received by subscription |

nav2_behavior_tree/CMakeLists.txt

@@ -101,6 +101,12 @@ list(APPEND plugin_libs nav2_distance_controller_bt_node)
 add_library(nav2_speed_controller_bt_node SHARED plugins/decorator/speed_controller.cpp)
 list(APPEND plugin_libs nav2_speed_controller_bt_node)
 
+add_library(nav2_truncate_path_action_bt_node SHARED plugins/action/truncate_path_action.cpp)
+list(APPEND plugin_libs nav2_truncate_path_action_bt_node)
+
+add_library(nav2_goal_updater_node_bt_node SHARED plugins/decorator/goal_updater_node.cpp)
+list(APPEND plugin_libs nav2_goal_updater_node_bt_node)
+
 add_library(nav2_recovery_node_bt_node SHARED plugins/control/recovery_node.cpp)
 list(APPEND plugin_libs nav2_recovery_node_bt_node)
 

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

@@ -0,0 +1,55 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Francisco Martin Rico
+//
+// 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__TRUNCATE_PATH_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_
+
+#include <memory>
+#include <string>
+
+#include "nav_msgs/msg/path.hpp"
+
+#include "behaviortree_cpp_v3/action_node.h"
+
+namespace nav2_behavior_tree
+{
+
+class TruncatePath : public BT::ActionNodeBase
+{
+public:
+  TruncatePath(
+    const std::string & xml_tag_name,
+    const BT::NodeConfiguration & conf);
+
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<nav_msgs::msg::Path>("input_path", "Original Path"),
+      BT::OutputPort<nav_msgs::msg::Path>("output_path", "Path truncated to a certain distance"),
+      BT::InputPort<double>("distance", 1.0, "distance"),
+    };
+  }
+
+private:
+  void halt() override {}
+  BT::NodeStatus tick() override;
+
+  double distance_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_

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

@@ -0,0 +1,61 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Francisco Martin Rico
+//
+// 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__DECORATOR__GOAL_UPDATER_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class GoalUpdater : public BT::DecoratorNode
+{
+public:
+  GoalUpdater(
+    const std::string & xml_tag_name,
+    const BT::NodeConfiguration & conf);
+
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<geometry_msgs::msg::PoseStamped>("input_goal", "Original Goal"),
+      BT::OutputPort<geometry_msgs::msg::PoseStamped>(
+        "output_goal",
+        "Received Goal by subscription"),
+    };
+  }
+
+private:
+  BT::NodeStatus tick() override;
+
+  void callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
+
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
+
+  geometry_msgs::msg::PoseStamped last_goal_received_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_

nav2_behavior_tree/plugins/action/truncate_path_action.cpp

@@ -0,0 +1,89 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Francisco Martin Rico
+//
+// 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 <limits>
+
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+#include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+TruncatePath::TruncatePath(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::ActionNodeBase(name, conf),
+  distance_(1.0)
+{
+  getInput("distance", distance_);
+}
+
+inline BT::NodeStatus TruncatePath::tick()
+{
+  setStatus(BT::NodeStatus::RUNNING);
+
+  nav_msgs::msg::Path input_path;
+
+  getInput("input_path", input_path);
+
+  if (input_path.poses.empty()) {
+    setOutput("output_path", input_path);
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  geometry_msgs::msg::PoseStamped final_pose = input_path.poses.back();
+
+  double distance_to_goal = nav2_util::geometry_utils::euclidean_distance(
+    input_path.poses.back(), final_pose);
+
+  while (distance_to_goal < distance_ && input_path.poses.size() > 2) {
+    input_path.poses.pop_back();
+    distance_to_goal = nav2_util::geometry_utils::euclidean_distance(
+      input_path.poses.back(), final_pose);
+  }
+
+  double dx = final_pose.pose.position.x - input_path.poses.back().pose.position.x;
+  double dy = final_pose.pose.position.y - input_path.poses.back().pose.position.y;
+
+  double final_angle = atan2(dy, dx);
+
+  if (std::isnan(final_angle) || std::isinf(final_angle)) {
+    RCLCPP_WARN(
+      config().blackboard->get<rclcpp::Node::SharedPtr>("node")->get_logger(),
+      "Final angle is not valid while truncating path. Setting to 0.0");
+    final_angle = 0.0;
+  }
+
+  input_path.poses.back().pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
+    final_angle);
+
+  setOutput("output_path", input_path);
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::TruncatePath>("TruncatePath");
+}

nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp

@@ -0,0 +1,71 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Francisco Martin Rico
+//
+// 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 "geometry_msgs/msg/pose_stamped.hpp"
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+#include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree
+{
+
+using std::placeholders::_1;
+
+GoalUpdater::GoalUpdater(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::DecoratorNode(name, conf)
+{
+  auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+
+  std::string goal_updater_topic;
+  node->get_parameter_or<std::string>("goal_updater_topic", goal_updater_topic, "goal_update");
+
+  goal_sub_ = node->create_subscription<geometry_msgs::msg::PoseStamped>(
+    goal_updater_topic, 10, std::bind(&GoalUpdater::callback_updated_goal, this, _1));
+}
+
+inline BT::NodeStatus GoalUpdater::tick()
+{
+  geometry_msgs::msg::PoseStamped goal;
+
+  getInput("input_goal", goal);
+
+  if (rclcpp::Time(last_goal_received_.header.stamp) > rclcpp::Time(goal.header.stamp)) {
+    goal = last_goal_received_;
+  }
+
+  setOutput("output_goal", goal);
+  return child_node_->executeTick();
+}
+
+void
+GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
+{
+  last_goal_received_ = *msg;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::GoalUpdater>("GoalUpdater");
+}

nav2_behavior_tree/test/plugins/action/CMakeLists.txt

@@ -29,3 +29,7 @@ ament_target_dependencies(test_action_follow_path_action ${dependencies})
 ament_add_gtest(test_action_navigate_to_pose_action test_navigate_to_pose_action.cpp)
 target_link_libraries(test_action_navigate_to_pose_action nav2_navigate_to_pose_action_bt_node)
 ament_target_dependencies(test_action_navigate_to_pose_action ${dependencies})
+
+ament_add_gtest(test_truncate_path_action test_truncate_path_action.cpp)
+target_link_libraries(test_truncate_path_action nav2_truncate_path_action_bt_node)
+ament_target_dependencies(test_truncate_path_action ${dependencies})