squash to remove unsigned commits

Signed-off-by: Teo Koon Peng <koonpeng@openrobotics.org>

nexus_integration_tests/config/workcell_1_bts/place_on_conveyor.xml

@@ -4,7 +4,7 @@
     <Sequence name="root_sequence">
       <dispense_item.DispenseItem name="dispense_item" item="productA" />
 
-      <TransformPoseLocal base_pose="pickup_pose" x="0" y="0" z="-0.02" qx="0" qy="0" qz="0" qw="1" result="{pickup_top_goal}" />
+      <ApplyTransform base_pose="pickup_pose" x="0" y="0" z="-0.02" qx="0" qy="0" qz="0" qw="1" local="true" result="{pickup_top_goal}" />
       <plan_motion.PlanMotion name="plan_to_pickup_top" robot_name="abb_irb910sc" goal="{pickup_top_goal}" scale_speed="0.5" cartesian="true" result="{pickup_top_traj}" />
       <GetJointConstraints trajectory="{pickup_top_traj}" joint_constraints="{joint_constraints}" />
       <execute_trajectory.ExecuteTrajectory name="execute_pickup_top" trajectory="{pickup_top_traj}" />

nexus_integration_tests/config/workcell_2_bts/pick_from_conveyor.xml

@@ -8,7 +8,7 @@
       <detection.GetDetection detections="{all_products}" idx="0" result="{current_product_detection}" />
       <detection.GetDetectionPose detection="{current_product_detection}" result="{current_product_pose}" />
 
-      <TransformPoseLocal base_pose="{current_product_pose}" x="0" y="0" z="-0.02" qx="0" qy="0" qz="0" qw="1" result="{pickup_top_goal}" />
+      <ApplyTransform base_pose="{current_product_pose}" x="0" y="0" z="-0.02" qx="0" qy="0" qz="0" qw="1" local="true" result="{pickup_top_goal}" />
       <plan_motion.PlanMotion name="plan_to_pickup_top" robot_name="abb_irb1300" goal="{pickup_top_goal}" scale_speed="0.5" result="{pickup_top_traj}" />
       <GetJointConstraints trajectory="{pickup_top_traj}" joint_constraints="{joint_constraints}" />
       <execute_trajectory.ExecuteTrajectory name="execute_pickup" trajectory="{pickup_top_traj}" />
@@ -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}" />
+      <ApplyTransform 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}" />

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="ApplyTransform">
+      <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="GetTransform">
+      <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, rclcpp::Time 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">

nexus_workcell_orchestrator/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(nexus_endpoints REQUIRED)
 find_package(nexus_lifecycle_manager REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(vision_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -55,6 +56,7 @@ target_link_libraries(${PROJECT_NAME}_plugin PUBLIC
   rclcpp_lifecycle::rclcpp_lifecycle
   rclcpp_components::component
   tf2::tf2
+  tf2_geometry_msgs::tf2_geometry_msgs
   tf2_ros::tf2_ros
   yaml-cpp
   ${vision_msgs_TARGETS}

nexus_workcell_orchestrator/package.xml

@@ -23,6 +23,8 @@
   <depend>rclcpp_action</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
   <depend>vision_msgs</depend>
   <depend>yaml-cpp</depend>
 

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<ApplyTransform>(
+    "ApplyTransform",
     [this](const std::string& name, const BT::NodeConfiguration& config)
     {
-      return std::make_unique<TransformPoseLocal>(name, config,
+      return std::make_unique<ApplyTransform>(name, config,
       *this->_node.lock(),
       this->_tf2_buffer);
     });

nexus_workcell_orchestrator/src/make_transform.hpp

@@ -23,7 +23,7 @@
 namespace nexus::workcell_orchestrator {
 
 /**
- * Creates a pose and write it to an output port
+ * Creates a transform and write it to an output port
  *
  * Input Ports:
  *   x |double|

nexus_workcell_orchestrator/src/transform_pose.cpp

@@ -22,11 +22,12 @@
 
 #include <rclcpp/logging.hpp>
 
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <tf2/LinearMath/Transform.h>
 
 namespace nexus::workcell_orchestrator {
 
-BT::NodeStatus TransformPoseLocal::tick()
+BT::NodeStatus ApplyTransform::tick()
 {
   auto base_pose = this->getInput<geometry_msgs::msg::PoseStamped>("base_pose");
   if (!base_pose)
@@ -43,43 +44,11 @@ BT::NodeStatus TransformPoseLocal::tick()
     base_pose->pose.position.z));
 
   tf2::Transform t;
-  tf2::Transform target_t = tf2::Transform::getIdentity();
-  auto maybe_pose = this->getInput<geometry_msgs::msg::Pose>(
-    "transform_from_pose");
-  auto maybe_pose_stamped = this->getInput<geometry_msgs::msg::PoseStamped>(
-    "transform_from_pose_stamped");
-
-  auto reverse = this->getInput<bool>("reverse").value_or(false);
-  if (maybe_pose_stamped)
-  {
-    const auto& pose = *maybe_pose_stamped;
-    t = this->_pose_to_tf(pose.pose, reverse);
-    try
-    {
-      auto transform_msg = this->_tf2_buffer->lookupTransform(
-        pose.header.frame_id, pose.header.stamp,
-        base_pose->header.frame_id, base_pose->header.stamp,
-        pose.header.frame_id);
-      target_t =
-        tf2::Transform(tf2::Quaternion(transform_msg.transform.rotation.x,
-          transform_msg.transform.rotation.y,
-          transform_msg.transform.rotation.z,
-          transform_msg.transform.rotation.w),
-          tf2::Vector3(transform_msg.transform.translation.x,
-          transform_msg.transform.translation.y,
-          transform_msg.transform.translation.z)) * base_trans.inverse();
-    }
-    catch (const tf2::TransformException& e)
-    {
-      RCLCPP_ERROR(
-        this->_node.get_logger(), "[%s]: Failed to get transform [%s]",
-        this->name().c_str(), e.what());
-      return BT::NodeStatus::FAILURE;
-    }
-  }
-  else if (maybe_pose)
+  auto maybe_tf = this->getInput<geometry_msgs::msg::Transform>("transform");
+
+  if (maybe_tf)
   {
-    t = this->_pose_to_tf(*maybe_pose);
+    tf2::fromMsg(*maybe_tf, t);
   }
   else
   {
@@ -104,7 +73,16 @@ BT::NodeStatus TransformPoseLocal::tick()
         tf2::Vector3(*x, *y, *z));
   }
 
-  auto result_trans = base_trans * target_t.inverse() * t * target_t;
+  tf2::Transform  result_trans;
+  const auto maybe_local = this->getInput<bool>("local");
+  if (maybe_local && *maybe_local)
+  {
+    result_trans = base_trans * t;
+  }
+  else
+  {
+    result_trans = t * base_trans;
+  }
   geometry_msgs::msg::PoseStamped result_pose = *base_pose;
   result_pose.pose.position.x = result_trans.getOrigin().x();
   result_pose.pose.position.y = result_trans.getOrigin().y();
@@ -124,25 +102,99 @@ BT::NodeStatus TransformPoseLocal::tick()
   return BT::NodeStatus::SUCCESS;
 }
 
-tf2::Transform TransformPoseLocal::_pose_to_tf(
-  const geometry_msgs::msg::Pose& pose, bool reverse)
+static tf2::Transform _pose_to_tf(const geometry_msgs::msg::Pose& pose)
 {
-  if (!reverse)
+  return tf2::Transform(tf2::Quaternion(pose.orientation.x,
+      pose.orientation.y, pose.orientation.z,
+      pose.orientation.w),
+      tf2::Vector3(pose.position.x, pose.position.y,
+      pose.position.z));
+}
+
+BT::NodeStatus GetTransform::tick()
+{
+  const auto base_pose = this->getInput<geometry_msgs::msg::PoseStamped>(
+    "base_pose");
+  if (!base_pose)
+  {
+    RCLCPP_ERROR(
+      this->_node.get_logger(), "[%s]: [base_pose] port is required",
+      this->name().c_str());
+    return BT::NodeStatus::FAILURE;
+  }
+  const auto base_tf = _pose_to_tf(base_pose->pose);
+  const auto target_pose = this->getInput<geometry_msgs::msg::PoseStamped>(
+    "target_pose");
+  if (!target_pose)
+  {
+    RCLCPP_ERROR(
+      this->_node.get_logger(), "[%s]: [target_pose] port is required",
+      this->name().c_str());
+    return BT::NodeStatus::FAILURE;
+  }
+  const auto target_tf = _pose_to_tf(target_pose->pose);
+
+  rclcpp::Time lookup_time;
+  const auto maybe_time = this->getInput<rclcpp::Time>("time");
+  if (maybe_time)
   {
-    return tf2::Transform(tf2::Quaternion(pose.orientation.x,
-        pose.orientation.y, pose.orientation.z,
-        pose.orientation.w),
-        tf2::Vector3(pose.position.x, pose.position.y,
-        pose.position.z));
+    lookup_time = *maybe_time;
   }
   else
   {
-    return tf2::Transform(tf2::Quaternion(pose.orientation.x,
-        pose.orientation.y, pose.orientation.z,
-        pose.orientation.w).inverse(),
-        tf2::Vector3(-pose.position.x, -pose.position.y,
-        -pose.position.z));
+    lookup_time = rclcpp::Time(0);  // Get latest available transforms
+  }
+
+  tf2::Transform frame_tf;
+  const auto handle_exception = [this](const std::exception& e)
+    {
+      RCLCPP_ERROR(this->_node.get_logger(), "%s", e.what());
+      return BT::NodeStatus::FAILURE;
+    };
+  try
+  {
+    const auto tf_msg = this->_tf2_buffer->lookupTransform(
+      base_pose->header.frame_id, target_pose->header.frame_id,
+      lookup_time);
+    tf2::fromMsg(tf_msg.transform, frame_tf);
   }
+  catch (const tf2::LookupException& e)
+  {
+    return handle_exception(e);
+  }
+  catch (const tf2::ConnectivityException& e)
+  {
+    return handle_exception(e);
+  }
+  catch (const tf2::ExtrapolationException& e)
+  {
+    return handle_exception(e);
+  }
+  catch (const tf2::InvalidArgumentException& e)
+  {
+    return handle_exception(e);
+  }
+
+  const auto maybe_local = this->getInput<bool>("local");
+  tf2::Transform result_tf;
+  if (maybe_local && *maybe_local)
+  {
+    result_tf = base_tf.inverse() * target_tf * frame_tf;
+  }
+  else
+  {
+    result_tf = target_tf * frame_tf * base_tf.inverse();
+  }
+  RCLCPP_INFO_STREAM(
+    this->_node.get_logger(),
+    "[" << this->name() << "]" << std::endl <<
+      "Base pose: " << base_pose->pose << " frame: " << base_pose->header.frame_id << std::endl <<
+      "Target pose: " << target_pose->pose << "frame: " << target_pose->header.frame_id << std::endl <<
+      "Result: " << result_tf;
+  );
+  this->setOutput("result", tf2::toMsg(result_tf));
+
+  return BT::NodeStatus::SUCCESS;
 }
 
 }