Improve interface of NavigateToPose

doc/parameters/param_list.md

@@ -597,8 +597,7 @@ When `recovery_plugins` parameter is not overridden, the following default plugi
 
 | Input Port | Default | Description |
 | ---------- | ------- | ----------- |
-| position | N/A | Position |
-| orientation | N/A | Orientation |
+| goal | N/A | Goal |
 | server_name | N/A | Action server name |
 | server_timeout | 10 | Action server timeout (ms) |
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp

@@ -15,9 +15,13 @@
 #ifndef NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_
 #define NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_
 
+#include <string>
+
+#include "rclcpp/time.hpp"
 #include "behaviortree_cpp_v3/behavior_tree.h"
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 
 namespace BT
 {
@@ -45,7 +49,7 @@ inline geometry_msgs::msg::Point convertFromString(const StringView key)
 template<>
 inline geometry_msgs::msg::Quaternion convertFromString(const StringView key)
 {
-  // three real numbers separated by semicolons
+  // four real numbers separated by semicolons
   auto parts = BT::splitString(key, ';');
   if (parts.size() != 4) {
     throw std::runtime_error("invalid number of fields for orientation attribute)");
@@ -59,6 +63,28 @@ inline geometry_msgs::msg::Quaternion convertFromString(const StringView key)
   }
 }
 
+template<>
+inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key)
+{
+  // 7 real numbers separated by semicolons
+  auto parts = BT::splitString(key, ';');
+  if (parts.size() != 9) {
+    throw std::runtime_error("invalid number of fields for PoseStamped attribute)");
+  } else {
+    geometry_msgs::msg::PoseStamped pose_stamped;
+    pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[0]));
+    pose_stamped.header.frame_id = BT::convertFromString<std::string>(parts[1]);
+    pose_stamped.pose.position.x = BT::convertFromString<double>(parts[2]);
+    pose_stamped.pose.position.y = BT::convertFromString<double>(parts[3]);
+    pose_stamped.pose.position.z = BT::convertFromString<double>(parts[4]);
+    pose_stamped.pose.orientation.x = BT::convertFromString<double>(parts[5]);
+    pose_stamped.pose.orientation.y = BT::convertFromString<double>(parts[6]);
+    pose_stamped.pose.orientation.z = BT::convertFromString<double>(parts[7]);
+    pose_stamped.pose.orientation.w = BT::convertFromString<double>(parts[8]);
+    return pose_stamped;
+  }
+}
+
 template<>
 inline std::chrono::milliseconds convertFromString<std::chrono::milliseconds>(const StringView key)
 {

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp

@@ -40,8 +40,7 @@ class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPo
   {
     return providedBasicPorts(
       {
-        BT::InputPort<geometry_msgs::msg::Point>("position", "Position"),
-        BT::InputPort<geometry_msgs::msg::Quaternion>("orientation", "Orientation")
+        BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
       });
   }
 };

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -28,8 +28,7 @@
     </Action>
 
     <Action ID="NavigateToPose">
-        <input_port name="orientation">Orientation</input_port>
-        <input_port name="position">Position</input_port>
+        <input_port name="goal">Goal</input_port>
     </Action>
 
     <Action ID="RandomCrawl"/>

nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp

@@ -30,19 +30,12 @@ NavigateToPoseAction::NavigateToPoseAction(
 
 void NavigateToPoseAction::on_tick()
 {
-  // Use the position and orientation fields from the XML attributes to initialize the goal
-  geometry_msgs::msg::Point position;
-  geometry_msgs::msg::Quaternion orientation;
-
-  if (!getInput("position", position) || !getInput("orientation", orientation)) {
+  if (!getInput("goal", goal_.pose)) {
     RCLCPP_ERROR(
       node_->get_logger(),
-      "NavigateToPoseAction: position or orientation not provided");
+      "NavigateToPoseAction: goal not provided");
     return;
   }
-
-  goal_.pose.pose.position = position;
-  goal_.pose.pose.orientation = orientation;
 }
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp

@@ -116,7 +116,7 @@ TEST_F(NavigateToPoseActionTestFixture, test_tick)
     R"(
       <root main_tree_to_execute = "MainTree" >
         <BehaviorTree ID="MainTree">
-            <NavigateToPose position="{position}" orientation="{orientation}" />
+            <NavigateToPose goal="{goal}" />
         </BehaviorTree>
       </root>)";
 
@@ -141,8 +141,7 @@ TEST_F(NavigateToPoseActionTestFixture, test_tick)
   // set new goal
   pose.pose.position.x = -2.5;
   pose.pose.orientation.x = 1.0;
-  config_->blackboard->set<geometry_msgs::msg::Point>("position", pose.pose.position);
-  config_->blackboard->set<geometry_msgs::msg::Quaternion>("orientation", pose.pose.orientation);
+  config_->blackboard->set<geometry_msgs::msg::PoseStamped>("goal", pose);
 
   EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
   EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose);

nav2_behavior_tree/test/test_bt_conversions.cpp

@@ -20,6 +20,7 @@
 
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 
 #include "behaviortree_cpp_v3/bt_factory.h"
 #include "nav2_behavior_tree/bt_conversions.hpp"
@@ -161,6 +162,81 @@ TEST(QuaternionPortTest, test_correct_syntax)
   EXPECT_EQ(value.w, 0.7);
 }
 
+TEST(PoseStampedPortTest, test_wrong_syntax)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <PoseStampedPort test="0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;8.0" />
+        </BehaviorTree>
+      </root>)";
+
+  BT::BehaviorTreeFactory factory;
+  factory.registerNodeType<TestNode<geometry_msgs::msg::PoseStamped>>("PoseStampedPort");
+  auto tree = factory.createTreeFromText(xml_txt);
+
+  geometry_msgs::msg::PoseStamped value;
+  tree.rootNode()->getInput("test", value);
+  EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0);
+  EXPECT_EQ(value.header.frame_id, "");
+  EXPECT_EQ(value.pose.position.x, 0.0);
+  EXPECT_EQ(value.pose.position.y, 0.0);
+  EXPECT_EQ(value.pose.position.z, 0.0);
+  EXPECT_EQ(value.pose.orientation.x, 0.0);
+  EXPECT_EQ(value.pose.orientation.y, 0.0);
+  EXPECT_EQ(value.pose.orientation.z, 0.0);
+  EXPECT_EQ(value.pose.orientation.w, 1.0);
+
+  xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <PoseStampedPort test="0;map;1.0;2.0;3.0;4.0;5.0;6.0" />
+        </BehaviorTree>
+      </root>)";
+
+  tree = factory.createTreeFromText(xml_txt);
+  tree.rootNode()->getInput("test", value);
+  EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0);
+  EXPECT_EQ(value.header.frame_id, "");
+  EXPECT_EQ(value.pose.position.x, 0.0);
+  EXPECT_EQ(value.pose.position.y, 0.0);
+  EXPECT_EQ(value.pose.position.z, 0.0);
+  EXPECT_EQ(value.pose.orientation.x, 0.0);
+  EXPECT_EQ(value.pose.orientation.y, 0.0);
+  EXPECT_EQ(value.pose.orientation.z, 0.0);
+  EXPECT_EQ(value.pose.orientation.w, 1.0);
+}
+
+TEST(PoseStampedPortTest, test_correct_syntax)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <PoseStampedPort test="0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0" />
+        </BehaviorTree>
+      </root>)";
+
+  BT::BehaviorTreeFactory factory;
+  factory.registerNodeType<TestNode<geometry_msgs::msg::PoseStamped>>("PoseStampedPort");
+  auto tree = factory.createTreeFromText(xml_txt);
+
+  tree = factory.createTreeFromText(xml_txt);
+  geometry_msgs::msg::PoseStamped value;
+  tree.rootNode()->getInput("test", value);
+  EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0);
+  EXPECT_EQ(value.header.frame_id, "map");
+  EXPECT_EQ(value.pose.position.x, 1.0);
+  EXPECT_EQ(value.pose.position.y, 2.0);
+  EXPECT_EQ(value.pose.position.z, 3.0);
+  EXPECT_EQ(value.pose.orientation.x, 4.0);
+  EXPECT_EQ(value.pose.orientation.y, 5.0);
+  EXPECT_EQ(value.pose.orientation.z, 6.0);
+  EXPECT_EQ(value.pose.orientation.w, 7.0);
+}
+
 TEST(MillisecondsPortTest, test_correct_syntax)
 {
   std::string xml_txt =