[feat] add python tf homework

learning_tf_homework/CMakeLists.txt

@@ -164,10 +164,10 @@ target_link_libraries(tf_listener_node
 
 ## Mark executable scripts (Python etc.) for installation
 ## in contrast to setup.py, you can choose the destination
-# catkin_install_python(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
+catkin_install_python(PROGRAMS
+  scripts/tf_broadcaster.py scripts/tf_listener.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
 
 ## Mark executables for installation
 ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html

learning_tf_homework/launch/start_py.launch

@@ -0,0 +1,18 @@
+<launch>
+
+<node
+    pkg="learning_tf_homework"
+    type="tf_broadcaster.py"
+    name="robot_tf_broadcaster"
+    required="true"
+    output="screen"
+/>
+<node
+    pkg="learning_tf_homework"
+    type="tf_listener.py"
+    name="robot_tf_listener"
+    required="true"
+    output="screen"
+/>
+
+</launch>

learning_tf_homework/scripts/tf_broadcaster.py

@@ -0,0 +1,20 @@
+#! /usr/bin/env python3
+
+import rospy
+import tf
+
+def main():
+  rospy.init_node("robot_tf_broadcaster")
+  
+  tf_br = tf.TransformBroadcaster()
+
+  rate = rospy.Rate(100)
+  while not rospy.is_shutdown():
+    translate_date = (0.1, 0.0, 0.2)
+    rotation_data = (0, 0, 0, 1)
+    tf_br.sendTransform(translate_date, rotation_data, rospy.Time.now(), "base_laser", "base_link")
+    
+    rate.sleep()
+
+if __name__ == "__main__":
+  main()

learning_tf_homework/scripts/tf_listener.py

@@ -0,0 +1,46 @@
+#! /usr/bin/env python3
+
+import rospy
+import tf
+import random
+from functools import partial
+
+from geometry_msgs.msg import PointStamped
+
+def tfPointCb(event, listener):
+  laser_point = PointStamped()
+  
+  laser_point.header.frame_id = "base_laser"
+  laser_point.header.stamp = rospy.Time(0)
+
+  laser_point.point.x = random.random()
+  laser_point.point.y = random.random()
+  laser_point.point.z = random.random()
+  
+  try:
+    (translation, rotation) = listener.lookupTransform("base_link", "base_laser", rospy.Time(0))    
+    rospy.loginfo("base_laser to base_link translation transform: (%.2f, %.2f, %.2f)" \
+          %(translation[0], translation[1], translation[2]))
+    
+    base_point = listener.transformPoint("base_link", laser_point)
+    rospy.loginfo("base_laser: (%.2f, %.2f, %.2f) --> base_link: (%.2f, %.2f, %.2f) at time %.2f" \
+            %(laser_point.point.x, laser_point.point.y, laser_point.point.z,
+            base_point.point.x, base_point.point.y, base_point.point.z,
+            base_point.header.stamp.to_sec()))
+  except Exception as e:
+    rospy.logerr("---> %s" %e)
+
+
+def main():
+  rospy.init_node("robot_tf_listener")
+  
+  tf_listener = tf.TransformListener(rospy.Duration(10))
+
+  bind_tf_point_cb = partial(tfPointCb, listener=tf_listener)
+
+  timer = rospy.Timer(rospy.Duration(1), bind_tf_point_cb)
+
+  rospy.spin()
+
+if __name__ == "__main__":
+  main()