Cleaned up version of is path valid

nav2_behavior_tree/CMakeLists.txt

@@ -84,9 +84,15 @@ list(APPEND plugin_libs nav2_transform_available_condition_bt_node)
 add_library(nav2_goal_reached_condition_bt_node SHARED plugins/condition/goal_reached_condition.cpp)
 list(APPEND plugin_libs nav2_goal_reached_condition_bt_node)
 
+add_library(nav2_globally_updated_goal_condition_bt_node SHARED plugins/condition/globally_updated_goal_condition.cpp)
+list(APPEND plugin_libs nav2_globally_updated_goal_condition_bt_node)
+
 add_library(nav2_goal_updated_condition_bt_node SHARED plugins/condition/goal_updated_condition.cpp)
 list(APPEND plugin_libs nav2_goal_updated_condition_bt_node)
 
+add_library(nav2_is_path_valid_condition_bt_node SHARED plugins/condition/is_path_valid_condition.cpp)
+list(APPEND plugin_libs nav2_is_path_valid_condition_bt_node)
+
 add_library(nav2_time_expired_condition_bt_node SHARED plugins/condition/time_expired_condition.cpp)
 list(APPEND plugin_libs nav2_time_expired_condition_bt_node)
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp

@@ -0,0 +1,73 @@
+// Copyright (c) 2021 Joshua Wallace
+//
+// 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__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_
+#define  NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_
+
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "behaviortree_cpp_v3/condition_node.h"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+
+namespace nav2_behavior_tree
+{
+/**
+ * @brief A BT::ConditionNode that returns SUCCESS when goal is
+ * updated on the blackboard and FAILURE otherwise
+ */
+class GloballyUpdatedGoalCondition : public BT::ConditionNode
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::GloballyUpdatedGoalCondition
+   * @param condition_name Name for the XML tag for this node
+   * @param conf BT node configuration
+   */
+  GloballyUpdatedGoalCondition(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf);
+
+  GloballyUpdatedGoalCondition() = delete;
+
+  /**
+   * @brief The main override required by a BT action
+   * @return BT::NodeStatus Status of tick execution
+   */
+  BT::NodeStatus tick() override;
+
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing node-specific ports
+   */
+  static BT::PortsList providedPorts()
+  {
+    return {};
+  }
+
+private:
+  bool first_time;
+  rclcpp::Node::SharedPtr node_;
+  geometry_msgs::msg::PoseStamped goal_;
+  std::vector<geometry_msgs::msg::PoseStamped> goals_;
+};
+
+}  // namespace nav2_behavior_tree
+
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp

@@ -0,0 +1,75 @@
+// Copyright (c) 2021 Joshua Wallace
+//
+// 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__CONDITION__IS_PATH_VALID_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_PATH_VALID_CONDITION_HPP_
+
+#include <string>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "behaviortree_cpp_v3/condition_node.h"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_msgs/srv/is_path_valid.hpp"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief A BT::ConditionNode that returns SUCCESS when the IsPathValid
+ * service returns true and FAILURE otherwise
+ */
+class IsPathValidCondition : public BT::ConditionNode
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::IsPathValidCondition
+   * @param condition_name Name for the XML tag for this node
+   * @param conf BT node configuration
+   */
+  IsPathValidCondition(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf);
+
+  IsPathValidCondition() = delete;
+
+  /**
+   * @brief The main override required by a BT action
+   * @return BT::NodeStatus Status of tick execution
+   */
+  BT::NodeStatus tick() override;
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing node-specific ports
+   */
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<nav_msgs::msg::Path>("path", "Path to Check"),
+      BT::InputPort<std::chrono::milliseconds>("server_timeout")
+    };
+  }
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Client<nav2_msgs::srv::IsPathValid>::SharedPtr client_;
+  // The timeout value while waiting for a responce from the
+  // is path valid service
+  std::chrono::milliseconds server_timeout_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_PATH_VALID_CONDITION_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -111,6 +111,8 @@
 
     <Condition ID="GoalUpdated"/>
 
+    <Condition ID="GloballyUpdatedGoalCondition"/>
+
     <Condition ID="IsBatteryLow">
       <input_port name="min_battery">Min battery % or voltage before triggering</input_port>
       <input_port name="battery_topic">Topic for battery info</input_port>
@@ -129,6 +131,12 @@
 
     <Condition ID="InitialPoseReceived">
     </Condition>
+
+    <Condition ID="IsPathValidCondition">
+      <input_port name="path"> Path to validate </input_port>
+      <input_port name="server_timeout"> Server timeout </input_port>
+    </Condition>
+
     <!-- ############################### CONTROL NODES ################################ -->
     <Control ID="PipelineSequence"/>
 

nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp

@@ -0,0 +1,61 @@
+// Copyright (c) 2021 Joshua Wallace
+//
+// 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 <vector>
+#include <string>
+
+#include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+GloballyUpdatedGoalCondition::GloballyUpdatedGoalCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  first_time(true)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+}
+
+BT::NodeStatus GloballyUpdatedGoalCondition::tick()
+{
+  if (first_time) {
+    first_time = false;
+    config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", goals_);
+    config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", goal_);
+    return BT::NodeStatus::FAILURE;
+  }
+
+  std::vector<geometry_msgs::msg::PoseStamped> current_goals;
+  config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", current_goals);
+  geometry_msgs::msg::PoseStamped current_goal;
+  config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", current_goal);
+
+  if (goal_ != current_goal || goals_ != current_goals) {
+    goal_ = current_goal;
+    goals_ = current_goals;
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  return BT::NodeStatus::FAILURE;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::GloballyUpdatedGoalCondition>("GlobalUpdatedGoal");
+}

nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp

@@ -23,8 +23,7 @@ GoalUpdatedCondition::GoalUpdatedCondition(
   const std::string & condition_name,
   const BT::NodeConfiguration & conf)
 : BT::ConditionNode(condition_name, conf)
-{
-}
+{}
 
 BT::NodeStatus GoalUpdatedCondition::tick()
 {

nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp

@@ -0,0 +1,61 @@
+// Copyright (c) 2021 Joshua Wallace
+//
+// 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 "nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp"
+#include <chrono>
+#include <memory>
+#include <string>
+
+namespace nav2_behavior_tree
+{
+
+IsPathValidCondition::IsPathValidCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  client_ = node_->create_client<nav2_msgs::srv::IsPathValid>("is_path_valid");
+
+  server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
+  getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+}
+
+BT::NodeStatus IsPathValidCondition::tick()
+{
+  nav_msgs::msg::Path path;
+  getInput("path", path);
+
+  auto request = std::make_shared<nav2_msgs::srv::IsPathValid::Request>();
+
+  request->path = path;
+  auto result = client_->async_send_request(request);
+
+  if (rclcpp::spin_until_future_complete(node_, result, server_timeout_) ==
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    if (result.get()->is_valid) {
+      return BT::NodeStatus::SUCCESS;
+    }
+  }
+  return BT::NodeStatus::FAILURE;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::IsPathValidCondition>("IsPathValid");
+}

nav2_behavior_tree/test/plugins/condition/CMakeLists.txt

@@ -14,6 +14,10 @@ ament_add_gtest(test_condition_goal_updated test_goal_updated.cpp)
 target_link_libraries(test_condition_goal_updated nav2_goal_updated_condition_bt_node)
 ament_target_dependencies(test_condition_goal_updated ${dependencies})
 
+ament_add_gtest(test_condition_globally_updated_goal test_globally_updated_goal.cpp)
+target_link_libraries(test_condition_globally_updated_goal nav2_globally_updated_goal_condition_bt_node)
+ament_target_dependencies(test_condition_globally_updated_goal ${dependencies})
+
 ament_add_gtest(test_condition_initial_pose_received test_initial_pose_received.cpp)
 target_link_libraries(test_condition_initial_pose_received nav2_initial_pose_received_condition_bt_node)
 ament_target_dependencies(test_condition_initial_pose_received ${dependencies})
@@ -29,3 +33,7 @@ ament_target_dependencies(test_condition_is_stuck ${dependencies})
 ament_add_gtest(test_condition_is_battery_low test_is_battery_low.cpp)
 target_link_libraries(test_condition_is_battery_low nav2_is_battery_low_condition_bt_node)
 ament_target_dependencies(test_condition_is_battery_low ${dependencies})
+
+ament_add_gtest(test_condition_is_path_valid test_is_path_valid.cpp)
+target_link_libraries(test_condition_is_path_valid nav2_is_path_valid_condition_bt_node)
+ament_target_dependencies(test_condition_is_path_valid ${dependencies})