[feat] add learning_tf

learning_tf/CMakeLists.txt

@@ -165,10 +165,10 @@ target_link_libraries(${PROJECT_NAME}_listener
 
 ## 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/turtle_tf_broadcaster.py scripts/turtle_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/launch/start_py.launch

@@ -0,0 +1,18 @@
+ <launch>
+
+    <!-- Turtlesim Node-->
+    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
+    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
+
+    <node pkg="learning_tf" type="turtle_tf_broadcaster.py" name="turtle1_tf_broadcaster" respawn="false" output="screen">
+      <param name="turtle" type="string" value="turtle1"/>
+    </node>
+    <node pkg="learning_tf" type="turtle_tf_broadcaster.py" name="turtle2_tf_broadcaster" respawn="false" output="screen">
+      <param name="turtle" type="string" value="turtle2"/>
+    </node>
+
+    <node pkg="learning_tf" type="learning_tf_listener" name="listener" />
+
+  </launch>
+
+  

learning_tf/scripts/turtle_tf_broadcaster.py

@@ -0,0 +1,35 @@
+#! /usr/bin/env python3
+
+import roslib
+import rospy
+from functools import partial
+
+import tf
+from turtlesim.msg import Pose
+
+
+def pose_cb(msg, tf_br, turtlename):
+  translate_date = (msg.x, msg.y, 0)
+  
+  # quaternion_from_euler直译过来就是来自欧拉角的四元数
+  rotation_data = tf.transformations.quaternion_from_euler(0, 0, msg.theta)
+  tf_br.sendTransform(translate_date, rotation_data, rospy.Time.now(), turtlename, "world")
+
+def main():
+  rospy.init_node("turtle_tf_broadcaster")
+  
+  # python版的tf广播器，利用全局参数传参
+  # rospy.get_param()是python的获取参数接口，cpp里是ros::param::get()
+  turtlename = rospy.get_param("~turtle")
+
+  pose_topic = "/%s/pose" %turtlename
+  tf_br = tf.TransformBroadcaster()
+  
+  bind_pose_cb = partial(pose_cb, tf_br=tf_br, turtlename=turtlename)
+  
+  rospy.Subscriber(pose_topic, Pose, bind_pose_cb)
+  
+  rospy.spin()
+
+if __name__ == "__main__":
+  main()

learning_tf/scripts/turtle_tf_listener.py

@@ -0,0 +1,48 @@
+#！/usr/bin/env python3
+
+import roslib
+import rospy
+import math
+import tf
+
+from geometry_msgs.msg import Twist
+from turtlesim.srv import Spawn
+
+def main():
+  rospy.init_node("turtle_tf_listener")
+  
+  rospy.wait_for_service("spawn")
+  client = rospy.ServiceProxy("spawn", Spawn)
+  
+  try:
+    req = SpawnRequest()
+    resp = client(req)
+    rospy.loginfo("spawned a turtle name %s" %resp.name)
+  except rospy.ServiceException as e:
+      rospy.loginfo("service call fail: %s" %e)
+
+  cmd_pub= rospy.Publisher("/turtle2/cmd_vel", Twist, queue_size=10)
+  
+  tf_listener = tf.TransformListener()
+  
+  rate = rospy.Rate(10)
+  while not rospy.is_shutdown():
+    try:
+      (translation, rotation) = tf_listener.lookupTransform("/turtle2", "turtle1", torpy.Time(0))
+    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+      continue
+
+    cmd_vel = Twist()
+    cmd_vel.linear.x= 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
+    cmd_vel.angular.z = 4 * math.atan2(translation[1], translation[0])
+
+    # 下面的改动，可以让turtle1控制turtle2的转向，即turtle2跟随turtle1转，但turtle2追不上turtle1
+    # quaternion = rotation
+    # # euler = (roll, pitch, yaw)
+    # euler = tf.transformations.euler_from_quaternion(quaternion)
+    # cmd_vel.angular.z = 4 * euler[2]
+    
+    cmd_pub.publish(cmd_vel)
+
+if __name__ == "__main__":
+  main()

learning_tf/src/turtle_tf_listener.cpp

@@ -38,8 +38,14 @@ int main(int argc, char ** argv) {
         }
 
         geometry_msgs::Twist vel;
-        vel.angular.z = 4.0 * atan2(tf_data.getOrigin().y(), tf_data.getOrigin().x());
         vel.linear.x = 0.5 * sqrt(pow(tf_data.getOrigin().x(), 2) + pow(tf_data.getOrigin().y(), 2));
+        vel.angular.z = 4.0 * atan2(tf_data.getOrigin().y(), tf_data.getOrigin().x());
+
+        // 下面的改动，可以让turtle1控制turtle2的转向，即turtle2跟随turtle1转，但turtle2追不上turtle1
+        // double roll, pitch, yaw;
+        // tf::Quaternion qua = tf_data.getRotation();
+        // tf::Matrix3x3(qua).getRPY(roll, pitch, yaw);
+        // vel.angular.z = 4.0 * yaw;
 
         cmd_pub.publish(vel);