enable offset parameter

code-iai · Sep 24, 2020 · fa3816c · fa3816c
1 parent ec38663
commit fa3816c
Showing 1 changed file with 32 additions and 3 deletions.
diff --git a/scripts/frame_in_other_frame_publisher.py b/scripts/frame_in_other_frame_publisher.py
@@ -15,6 +15,16 @@ def publisher(args):
     rate = rospy.Rate(10.0)
     while not rospy.is_shutdown():
         try:
+            correction_transform_x = rospy.get_param("offset_translation_x")
+            correction_transform_y = rospy.get_param("offset_translation_y")
+            correction_transform_z = rospy.get_param("offset_translation_z")
+
+            correction_rotation_r = rospy.get_param("offset_rotation_r")
+            correction_rotation_p = rospy.get_param("offset_rotation_p")
+            correction_rotation_y = rospy.get_param("offset_rotation_y")
+
+            apply_offset = rospy.get_param("apply_offset")
+
             (trans,rot) = listener.lookupTransform(args.target_frame, 
                     args.src_frame, rospy.Time(0))
             # print(".")
@@ -24,18 +34,28 @@ def publisher(args):
             t.header.frame_id = args.target_frame
             t.child_frame_id = args.src_frame
 
-            t.transform.translation.x = trans[0]
-            t.transform.translation.y = trans[1]
-            t.transform.translation.z = trans[2]
+            if apply_offset:
+                t.transform.translation.x = trans[0] + correction_transform_x
+                t.transform.translation.y = trans[1] + correction_transform_y
+                t.transform.translation.z = trans[2] + correction_transform_z
+            else:
+                t.transform.translation.x = trans[0]
+                t.transform.translation.y = trans[1]
+                t.transform.translation.z = trans[2]
 
             t.transform.rotation.x = rot[0]
             t.transform.rotation.y = rot[1]
             t.transform.rotation.z = rot[2]
             t.transform.rotation.w = rot[3]
 
+            # correction_rotation = tf.transformations.quaternion_from_euler(correction_rotation_r, correction_rotation_p, correction_rotation_y)
+            # # t.transform.rotation = t.transform.rotation * correction_rotation
+            # t.transform.rotation = tf.transformations.quaternion_multiply(t.transform.rotation,correction_rotation)
+
             tfm = tf2_msgs.msg.TFMessage([t])
             # print(tfm)
             pub.publish(tfm)
+            rate.sleep()
 
 
         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
@@ -59,6 +79,15 @@ def publisher(args):
     print("Target frame: " + args.target_frame)
     print("Topic: " + args.topic)
 
+    rospy.set_param("offset_translation_x",0)
+    rospy.set_param("offset_translation_y",0)
+    rospy.set_param("offset_translation_z",0)
+
+    rospy.set_param("offset_rotation_r",0)
+    rospy.set_param("offset_rotation_p",0)
+    rospy.set_param("offset_rotation_y",0)
+
+    rospy.set_param("apply_offset", True)
 
     try:
         publisher(args)