From 929261634648190e566fc3b0523fc7e6c2483795 Mon Sep 17 00:00:00 2001
From: Marina Moreira <67443181+marinagmoreira@users.noreply.github.com>
Date: Tue, 21 Nov 2023 17:38:34 -0800
Subject: [PATCH] Cargo transform config (#106)

* making spawn changes and adding double handed cargo

* adding configurable transform + better instructions with demo
---
 astrobee/behaviors/cargo/readme.md         | 15 ++++++++
 astrobee/behaviors/cargo/src/cargo_node.cc | 40 ----------------------
 isaac/config/transforms.config             | 17 +++++++++
 3 files changed, 32 insertions(+), 40 deletions(-)

diff --git a/astrobee/behaviors/cargo/readme.md b/astrobee/behaviors/cargo/readme.md
index 46511381..55df4acb 100644
--- a/astrobee/behaviors/cargo/readme.md
+++ b/astrobee/behaviors/cargo/readme.md
@@ -17,3 +17,18 @@ drop - drops the cargo
 The options that can be defined are:
 cargo_pose     - a pose defined in quaternions of the cargo bag
 
+# Demo example
+
+Launch isaac simulation normally
+
+Spawn cargo:
+
+    roslaunch isaac_gazebo spawn_object.launch spawn:=cargo pose:="11.3 -5.6 5.6 -0.707 0 0 0.707" name:=CTB_05_1070
+
+Pick up cargo (make sure astrobee is undocked) - the pose is the cargo pose inserted above:
+
+    rosrun cargo cargo_tool -id CTB_05_1070 -pick -pose "11.3 -5.6 5.6 -0.707 0 0 0.707"
+
+Drop cargo - the pose is the dock pose:
+
+    rosrun cargo cargo_tool -drop -pose "10.4 -5.6 5.855 -0.707 0 0 0.707"
\ No newline at end of file
diff --git a/astrobee/behaviors/cargo/src/cargo_node.cc b/astrobee/behaviors/cargo/src/cargo_node.cc
index 20e32568..b2aa807b 100755
--- a/astrobee/behaviors/cargo/src/cargo_node.cc
+++ b/astrobee/behaviors/cargo/src/cargo_node.cc
@@ -391,46 +391,6 @@ class CargoNode : public ff_util::FreeFlyerNodelet {
     server_.SetCancelCallback(std::bind(
       &CargoNode::CancelCallback, this));
     server_.Create(nh, ACTION_BEHAVIORS_CARGO);
-
-
-    geometry_msgs::TransformStamped tf;
-    tf.header.stamp = ros::Time::now();
-
-    // Trasform from cargo/berth to cargo/body
-    tf.transform.translation =
-      msg_conversions::eigen_to_ros_vector(Eigen::Vector3d(0, 0.255, 0));
-    tf.transform.rotation =
-      msg_conversions::eigen_to_ros_quat(Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0));
-    tf.header.frame_id = std::string("cargo/berth");
-    tf.child_frame_id = std::string("cargo/body");
-    bc_.sendTransform(tf);
-    tf.header.frame_id = std::string("cargo_goal/berth");
-    tf.child_frame_id = std::string("cargo_goal/body");
-    bc_.sendTransform(tf);
-
-    // Trasform from cargo/body to cargo/approach
-    tf.transform.translation =
-      msg_conversions::eigen_to_ros_vector(Eigen::Vector3d(0.1, 1.1, 0.15));
-    tf.transform.rotation =
-      msg_conversions::eigen_to_ros_quat(Eigen::Quaterniond(0.707, 0, 0, 0.707));
-    tf.header.frame_id = std::string("cargo/body");
-    tf.child_frame_id = std::string("cargo/approach");
-    bc_.sendTransform(tf);
-    tf.header.frame_id = std::string("cargo_goal/body");
-    tf.child_frame_id = std::string("cargo_goal/approach");
-    bc_.sendTransform(tf);
-
-    // Trasform from cargo/body to cargo/complete
-    tf.transform.translation =
-      msg_conversions::eigen_to_ros_vector(Eigen::Vector3d(0.1, 0.6, 0.15));
-    tf.transform.rotation =
-      msg_conversions::eigen_to_ros_quat(Eigen::Quaterniond(0.707, 0, 0, 0.707));
-    tf.header.frame_id = std::string("cargo/body");
-    tf.child_frame_id = std::string("cargo/complete");
-    bc_.sendTransform(tf);
-    tf.header.frame_id = std::string("cargo_goal/body");
-    tf.child_frame_id = std::string("cargo_goal/complete");
-    bc_.sendTransform(tf);
   }
 
   // Ensure all clients are connected
diff --git a/isaac/config/transforms.config b/isaac/config/transforms.config
index 7fe1a03a..1437e493 100644
--- a/isaac/config/transforms.config
+++ b/isaac/config/transforms.config
@@ -32,6 +32,23 @@ transforms = {
     { global = false, parent = "cam",  child = "target",
       transform = transform(vec3(0.0, 0.0, 0.0), quat4(0.500, -0.500, -0.500, 0.500) ) },
 
+    -- CARGO
+
+    { global = false, parent = "cargo_goal/berth",  child = "cargo_goal/body",
+      transform = transform(vec3(0.0, 0.255, 0.0), quat4(0.0, 0.0, 0.0, 1.0) ) },
+    { global = false, parent = "cargo/berth",  child = "cargo/body",
+      transform = transform(vec3(0.0, 0.255, 0.0), quat4(0.0, 0.0, 0.0, 1.0) ) },
+
+    { global = false, parent = "cargo_goal/body",  child = "cargo_goal/approach",
+      transform = transform(vec3(0.1, 1.1, 0.15), quat4(0.0, 0.0, 0.707, 0.707) ) },
+    { global = false, parent = "cargo/body",  child = "cargo/approach",
+      transform = transform(vec3(0.1, 1.1, 0.15), quat4(0.0, 0.0, 0.707, 0.707) ) },
+
+    { global = false, parent = "cargo_goal/body",  child = "cargo_goal/complete",
+      transform = transform(vec3(0.1, 0.6, 0.15), quat4(0.0, 0.0, 0.707, 0.707) ) },
+    { global = false, parent = "cargo/body",  child = "cargo/complete",
+      transform = transform(vec3(0.1, 0.6, 0.15), quat4(0.0, 0.0, 0.707, 0.707) ) },
+
     -------------
     -- GLOBALS --
     -------------