Cargo transform config (#106)

* making spawn changes and adding double handed cargo

* adding configurable transform + better instructions with demo

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"

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

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 --
     -------------