diff --git a/learning_tf_homework/CMakeLists.txt b/learning_tf_homework/CMakeLists.txt index 210d0f2..7d407c1 100644 --- a/learning_tf_homework/CMakeLists.txt +++ b/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 diff --git a/learning_tf_homework/launch/start_py.launch b/learning_tf_homework/launch/start_py.launch new file mode 100644 index 0000000..5f5d55f --- /dev/null +++ b/learning_tf_homework/launch/start_py.launch @@ -0,0 +1,18 @@ + + + + + + \ No newline at end of file diff --git a/learning_tf_homework/scripts/tf_broadcaster.py b/learning_tf_homework/scripts/tf_broadcaster.py new file mode 100644 index 0000000..50499fb --- /dev/null +++ b/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() diff --git a/learning_tf_homework/scripts/tf_listener.py b/learning_tf_homework/scripts/tf_listener.py new file mode 100644 index 0000000..b4bdd95 --- /dev/null +++ b/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()