Feature/update noisy tf (#37)

* only trigger ci on main for now

* add parameter to control noise and fix shutdown behavior

Author: nathanhhughes

.github/workflows/ci.yaml

@@ -1,7 +1,7 @@
 ---
 # based on https://github.com/actions/starter-workflows/blob/main/ci/cmake-single-platform.yml
 name: Hydra Build and Test
-on: {push: {branches: [main, develop]}, pull_request: {}}
+on: {push: {branches: [main]}, pull_request: {branches: [main]}}
 jobs:
   lint:
     runs-on: ubuntu-latest

hydra_ros/app/noisy_tf_publisher

@@ -72,6 +72,7 @@ class NoisyTransformPublisher(Node):
             "wrong_velocity_frame", False
         ).bool_value
         self._start_from_origin = self._get_param("start_from_origin", True).bool_value
+        self._add_noise = self._get_param("add_noise", True).bool_value
         self._parent_frame = self._get_param("parent_frame", "odom").string_value
         self._child_frame = self._get_param("child_frame", "base_link").string_value
         self._br = tf2_ros.TransformBroadcaster(self)
@@ -124,9 +125,12 @@ class NoisyTransformPublisher(Node):
         updated_odom.header = cur_gt_odom.header
 
         prev_pose = _mat_from_pose(prev_noisy_odom.pose.pose)
-        noisy_trans = np.random.normal(scale=self._xyz_std_dev)
-        noisy_rot = [0, 0, np.random.normal(scale=self._yaw_std_dev)]
-        updated_pose = prev_pose @ pose_delta @ Exp(noisy_trans, noisy_rot)
+        updated_pose = prev_pose @ pose_delta
+        if self._add_noise:
+            noisy_trans = np.random.normal(scale=self._xyz_std_dev)
+            noisy_rot = [0, 0, np.random.normal(scale=self._yaw_std_dev)]
+            updated_pose @= Exp(noisy_trans, noisy_rot)
+
         updated_odom.pose.pose.position.x = updated_pose[0, 3]
         updated_odom.pose.pose.position.y = updated_pose[1, 3]
         updated_odom.pose.pose.position.z = updated_pose[2, 3]
@@ -166,10 +170,13 @@ class NoisyTransformPublisher(Node):
 
 def main(args=None):
     rclpy.init()
-    node = NoisyTransformPublisher()
-    rclpy.spin(node)
-    node.destroy_node()
-    rclpy.shutdown()
+    try:
+        node = NoisyTransformPublisher()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        rclpy.try_shutdown()
 
 
 if __name__ == "__main__":