Add files via upload

src/frame_a_to_frame_b_broadcaster.py

@@ -0,0 +1,37 @@
+#!/usr/bin/env python  
+import roslib
+import rospy
+import tf
+import time  
+import math
+import time
+
+if __name__ == '__main__':
+	#init the node
+    rospy.init_node('frame_a_frame_b_broadcaster_node')
+
+    time.sleep(2)
+    #create a transformation broadcaster (publisher)
+    transform_broadcaster = tf.TransformBroadcaster()
+
+    while(not rospy.is_shutdown()):
+
+        #create a quaternion
+        rotation_quaternion = tf.transformations.quaternion_from_euler(0.2, 0.3, 0.1)
+
+        #translation vector
+        translation_vector = (1.0, 2.0, 3.0)
+
+        #time
+        current_time = rospy.Time.now()
+
+        transform_broadcaster.sendTransform(
+            translation_vector, 
+            rotation_quaternion,
+            current_time, 
+            "frame_b", "frame_a") #child frame, parent frame
+        time.sleep(0.5)
+
+    rospy.spin()
+
+

src/frame_a_to_frame_b_listener.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python  
+import rospy
+import math
+import tf
+import geometry_msgs.msg
+import turtlesim.srv
+
+if __name__ == '__main__':
+    rospy.init_node('frame_a_frame_b_listener_node')
+
+    listener = tf.TransformListener()
+    rate = rospy.Rate(1.0)
+    listener.waitForTransform('/frame_a', '/frame_b', rospy.Time(), rospy.Duration(4.0))
+
+    while not rospy.is_shutdown():
+        try:
+            (trans,rot) = listener.lookupTransform('/frame_a', '/frame_b', rospy.Time(0))
+        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+            continue
+
+        quaternion = rot
+        rpy=tf.transformations.euler_from_quaternion(quaternion)
+        print 'transformation between frame_a and frame_b detected'
+        print 'translation vector: (',trans[0],',',trans[1],',',trans[2],')'
+        print 'rotation angles: roll=',rpy[0],' pitch=',rpy[1],' yaw=',rpy[2]
+
+
+        rate.sleep()

src/tf_orientation_tb3_robot.py

@@ -0,0 +1,61 @@
+#!/usr/bin/env python
+
+#this is a simple program that subscribes to the odom topic and gets the position and orientation of the robot
+import rospy
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+import math
+import time
+from std_srvs.srv import Empty
+import tf
+
+#callback function the odom pose (position+orientation) of the robot 
+def odomPoseCallback(odom_msg):
+
+    print "odom pose callback"
+    #get the position of the robot
+    print 'x = ',odom_msg.pose.pose.position.x
+    print 'y = ', odom_msg.pose.pose.position.y 
+    #get the velocity of the robot
+    print 'vx = ',odom_msg.twist.twist.linear.x
+    print 'vz = ',odom_msg.twist.twist.angular.z
+
+    #print the values of the orientation in quaternion
+    print 'qx=',odom_msg.pose.pose.orientation.x
+    print 'qy=',odom_msg.pose.pose.orientation.y
+    print 'qz=',odom_msg.pose.pose.orientation.z
+    print 'qw=',odom_msg.pose.pose.orientation.w
+
+    #formulate a quaternion as a list
+    quaternion = (
+    odom_msg.pose.pose.orientation.x,
+    odom_msg.pose.pose.orientation.y,
+    odom_msg.pose.pose.orientation.z,
+    odom_msg.pose.pose.orientation.w)
+
+    #convert the quaternion to roll-pitch-yaw
+    rpy = tf.transformations.euler_from_quaternion(quaternion)
+    #extract the values of roll, pitch and yaw from the array
+    roll = rpy[0]
+    pitch = rpy[1]
+    yaw = rpy[2]
+
+    #print the roll, pitch and yaw
+    print math.degrees(roll), ' ', math.degrees(pitch), ' ', math.degrees(yaw)
+    print 'the orientation of the robot is: ',math.degrees(yaw)
+
+
+
+if __name__ == '__main__':
+    try:
+        #init the node
+        rospy.init_node('turtlesim_motion_pose', anonymous=True)
+
+       #subscribe to the odom topic 
+        position_topic = "/odom"
+        pose_subscriber = rospy.Subscriber(position_topic, Odometry, odomPoseCallback) 
+        #spin
+        rospy.spin()
+
+    except rospy.ROSInterruptException:
+        rospy.loginfo("node terminated.")

src/tf_rotation_conversions.py

@@ -0,0 +1,61 @@
+#!/usr/bin/env python
+
+#this script converts between Roll-Pitch-Yaw angles and Quaternions
+
+import rospy
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+import math
+import tf
+
+print('-----------------------------------------')
+print('Roll-Pitch-Yaw Conversion to Quaternions')
+print('-----------------------------------------')
+#we define some random angles roll, pitch and yaw in radians
+roll = math.radians(30)
+pitch = math.radians(42)
+yaw = math.radians(58)
+
+#we print these three angles
+print 'roll = ', math.degrees(roll), 'pitch = ', math.degrees(pitch),'yaw = ', math.degrees(yaw)
+
+#convert the roll-pitch-yaw angles to a quaternion using ROS TF Library
+quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
+print('-----------------------------------------')
+print 'The resulting quaternion using "quaternion_from_euler" function: '
+for i in range(4):
+	print quaternion[i]
+
+
+#quaternion[0] = -3.88256895463e-06
+#quaternion[1] = 0.0015896463485
+#quaternion[2] = 0.001397167245
+#quaternion[3] = 0.999997760464
+
+#perform the opposite converion
+rpy = tf.transformations.euler_from_quaternion(quaternion)
+roll_from_quaternion = rpy[0]
+pitch_from_quaternion = rpy[1]
+yaw_from_quaternion = rpy[2]
+
+print('-----------------------------------------')
+print 'convert back to roll-pitch-yaw using "euler_from_quaternion" function: '
+print 'roll = ', math.degrees(roll_from_quaternion), 'pitch = ', math.degrees(pitch_from_quaternion),'yaw = ', math.degrees(yaw_from_quaternion)
+print('we get the same initial roll-picth-roll values')
+print('-----------------------------------------')
+#define a new quaternion
+print('define a new quaternion manually as a list')
+q=(-3.88256895463e-06, 0.0015896463485, 0.001397167245, 0.0)
+
+#perform the opposite converion
+rpy = tf.transformations.euler_from_quaternion(q)
+roll_from_quaternion = rpy[0]
+pitch_from_quaternion = rpy[1]
+yaw_from_quaternion = rpy[2]
+print('-----------------------------------------')
+print 'convert back to roll-pitch-yaw using "euler_from_quaternion" function: '
+print 'roll = ', math.degrees(roll_from_quaternion), 'pitch = ', math.degrees(pitch_from_quaternion),'yaw = ', math.degrees(yaw_from_quaternion)
+print('-----------------------------------------')
+print('you can change these values in the file to make other converions')
+
+