From 782976a2ef062ed0c79662a273e301f24a769d7b Mon Sep 17 00:00:00 2001
From: methylDragon <methyldragon@google.com>
Date: Wed, 23 Aug 2023 11:31:44 -0700
Subject: [PATCH] Clean up lingering TransformPoseLocals

Signed-off-by: methylDragon <methylDragon@gmail.com>
---
 .../workcell_2_bts/pick_from_conveyor.xml     |  2 +-
 nexus_tree_nodes.xml                          | 19 +++++++++++++------
 nexus_workcell_orchestrator/src/job.cpp       |  6 +++---
 3 files changed, 17 insertions(+), 10 deletions(-)

diff --git a/nexus_integration_tests/config/workcell_2_bts/pick_from_conveyor.xml b/nexus_integration_tests/config/workcell_2_bts/pick_from_conveyor.xml
index 2523588..059877d 100644
--- a/nexus_integration_tests/config/workcell_2_bts/pick_from_conveyor.xml
+++ b/nexus_integration_tests/config/workcell_2_bts/pick_from_conveyor.xml
@@ -18,7 +18,7 @@
       <execute_trajectory.ExecuteTrajectory name="execute_pickup" trajectory="{pickup_traj}" />
       <gripper.GripperControl name="pickup_product" gripper="workcell_2_mock_gripper" position="0.0" max_effort="1.0" />
 
-      <TransformPoseLocal base_pose="dropoff_pose" x="0" y="0" z="-0.02" qx="0" qy="0" qz="0" qw="1" result="{dropoff_top_goal}" />
+      <ApplyPoseOffset base_pose="dropoff_pose" x="0" y="0" z="-0.02" qx="0" qy="0" qz="0" qw="1" result="{dropoff_top_goal}" />
       <plan_motion.PlanMotion name="plan_to_dropoff_top" robot_name="abb_irb1300" goal="{dropoff_top_goal}" scale_speed="0.5" result="{dropoff_top_traj}" />
       <GetJointConstraints trajectory="{dropoff_top_traj}" joint_constraints="{joint_constraints}" />
       <execute_trajectory.ExecuteTrajectory name="execute_dropoff_top" trajectory="{dropoff_top_traj}" />
diff --git a/nexus_tree_nodes.xml b/nexus_tree_nodes.xml
index 559a2ad..777fe10 100644
--- a/nexus_tree_nodes.xml
+++ b/nexus_tree_nodes.xml
@@ -121,8 +121,8 @@
       <input_port name="signal">An std::string signal to set within a workcell</input_port>
     </Action>
 
-    <Action ID="TransformPoseLocal">
-      <input_port name="base_pose">The pose to apply input offsets to</input_port>
+    <Action ID="ApplyPoseOffset">
+      <input_port name="base_pose">The geometry_msgs::msg::PoseStamped pose to apply input offsets to</input_port>
       <input_port name="x">The translation along X-Axis</input_port>
       <input_port name="y">The translation along Y-Axis</input_port>
       <input_port name="y">The translation along Y-Axis</input_port>
@@ -131,10 +131,17 @@
       <input_port name="qy">The qy component of quaternion rotation</input_port>
       <input_port name="qz">The qz component of quaternion rotation</input_port>
       <input_port name="qw">The qw component of quaternion rotation</input_port>
-      <input_port name="transform_from_pose">A geometry_msgs::msg::Pose which defines the desired transfrom</input_port>
-      <input_port name="transform_from_pose_stamped">Takes precedence over transform_from_pose</input_port>
-      <input_port name="reverse">Applies the mirror of the transform. Defaults to false</input_port>
-      <output_port name="result">The desired transform as a geometry_msgs::msg::Transform</output_port>
+      <input_port name="transform">A geometry_msgs::msg::Transform which defines the desired transform. Overrides the direct values if defined.</input_port>
+      <input_port name="local">Perform a local transform instead, wrt. the frame of the base_pose. Defaults to false</input_port>
+      <output_port name="result">The pose with the offset applied as a geometry_msgs::msg::PoseStamped</output_port>
+    </Action>
+
+    <Action ID="GetPoseOffset">
+      <input_port name="base_pose">The geometry_msgs::msg::PoseStamped pose at the start of the offset</input_port>
+      <input_port name="target_pose">The geometry_msgs::msg::PoseStamped pose at the end of the offset</input_port>
+      <input_port name="time">OPTIONAL, timepoint to lookup on, if not provided, the current time is used</input_port>
+      <input_port name="local">OPTIONAL, if true, return t such that `target_pose = base_pose * t`</input_port>
+      <output_port name="result">The geometry_msgs::msg::Transform offset that transforms base_pose to target_pose</input_port>
     </Action>
 
     <Action ID="detection.GetDetection">
diff --git a/nexus_workcell_orchestrator/src/job.cpp b/nexus_workcell_orchestrator/src/job.cpp
index 7597e37..ade55c9 100644
--- a/nexus_workcell_orchestrator/src/job.cpp
+++ b/nexus_workcell_orchestrator/src/job.cpp
@@ -58,11 +58,11 @@ Job::Job(rclcpp_lifecycle::LifecycleNode::WeakPtr node,
     });
 
   this->_bt_factory.registerNodeType<MakeTransform>("MakeTransform");
-  this->_bt_factory.registerBuilder<TransformPoseLocal>(
-    "TransformPoseLocal",
+  this->_bt_factory.registerBuilder<ApplyPoseOffset>(
+    "ApplyPoseOffset",
     [this](const std::string& name, const BT::NodeConfiguration& config)
     {
-      return std::make_unique<TransformPoseLocal>(name, config,
+      return std::make_unique<ApplyPoseOffset>(name, config,
       *this->_node.lock(),
       this->_tf2_buffer);
     });