$( \u2026 )<\/code>\u662f Linux \u7ec8\u7aef\u4e2d\u7684\u547d\u4ee4\u66ff\u6362\u7b26\u3002\u5b83\u4f1a\u5148\u6267\u884c\u62ec\u53f7\u5185\u7684\u547d\u4ee4\uff0c\u7136\u540e\u5c06\u8be5\u547d\u4ee4\u7684\u8f93\u51fa\u7ed3\u679c\u201c\u586b\u5145\u201d\u5230\u5f53\u524d\u4f4d\u7f6e\u3002\u6700\u540e\uff0c\u5728\u5f97\u5230\u7684 share \u76ee\u5f55\u7684\u8def\u5f84\u540e\u9762\uff0c\u518d\u63a5\u4e0a urtle_tf2_py \u5305\u5185 rviz \u5b50\u6587\u4ef6\u5939\u4e0b\u7684 turtle_rviz.rviz \u914d\u7f6e\u6587\u4ef6\u8def\u5f84\u3002<\/p>\n\n\n\n\u5c0f\u6d77\u9f9f\u8fd0\u52a8\u65f6\uff0cRviz\u4e2d\u7684\u5750\u6807\u8f74\u5c31\u4f1a\u5f00\u59cb\u8fd0\u52a8\u3002<\/p>\n\n\n\n
<\/p>\n\n\n\n
<\/p>\n\n\n\n
\u9759\u6001TF\u53d8\u6362<\/strong>\uff1a\u76f8\u5bf9\u4f4d\u7f6e\u4e0d\u53d1\u751f\u53d8\u5316\u7684\u60c5\u51b5\uff0c\u5982\u6fc0\u5149\u96f7\u8fbe\u548c\u673a\u5668\u4eba\u5e95\u76d8\u4e4b\u95f4\u7684\u4f4d\u7f6e\u5173\u7cfb\uff0c\u5b89\u88c5\u597d\u4e4b\u540e\u57fa\u672c\u4e0d\u4f1a\u53d8\u5316\u3002<\/p>\n\n\n\n\u4e00\u4e2a\u793a\u4f8b\u7684\u4ee3\u7801\u5982\u4e0b\uff1a<\/p>\n\n\n\n
import rclpy # ROS2 Python\u63a5\u53e3\u5e93\nfrom rclpy.node import Node # ROS2 \u8282\u70b9\u7c7b\nfrom geometry_msgs.msg import TransformStamped # \u5750\u6807\u53d8\u6362\u6d88\u606f\nimport tf_transformations # TF\u5750\u6807\u53d8\u6362\u5e93\nfrom tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster # TF\u9759\u6001\u5750\u6807\u7cfb\u5e7f\u64ad\u5668\u7c7b\n\nclass StaticTFBroadcaster(Node):\n def __init__(self, name):\n super().__init__(name) # ROS2\u8282\u70b9\u7236\u7c7b\u521d\u59cb\u5316\n self.tf_broadcaster = StaticTransformBroadcaster(self) # \u521b\u5efa\u4e00\u4e2aTF\u5e7f\u64ad\u5668\u5bf9\u8c61\n\n static_transformStamped = TransformStamped() # \u521b\u5efa\u4e00\u4e2a\u5750\u6807\u53d8\u6362\u7684\u6d88\u606f\u5bf9\u8c61\uff0c\u8fd9\u662fROS2\u5185\u7684\u6807\u51c6\u5b9a\u4e49\u7c7b\n static_transformStamped.header.stamp = self.get_clock().now().to_msg() # \u8bbe\u7f6e\u5750\u6807\u53d8\u6362\u6d88\u606f\u7684\u65f6\u95f4\u6233\n static_transformStamped.header.frame_id = 'world' # \u8bbe\u7f6e\u4e00\u4e2a\u5750\u6807\u53d8\u6362\u7684\u6e90\u5750\u6807\u7cfb\n static_transformStamped.child_frame_id = 'house' # \u8bbe\u7f6e\u4e00\u4e2a\u5750\u6807\u53d8\u6362\u7684\u76ee\u6807\u5750\u6807\u7cfb\n static_transformStamped.transform.translation.x = 10.0 # \u8bbe\u7f6e\u5750\u6807\u53d8\u6362\u4e2d\u7684X\u3001Y\u3001Z\u5411\u7684\u5e73\u79fb\n static_transformStamped.transform.translation.y = 5.0 \n static_transformStamped.transform.translation.z = 0.0\n quat = tf_transformations.quaternion_from_euler(0.0, 0.0, 0.0) # \u5c06\u6b27\u62c9\u89d2\u8f6c\u6362\u4e3a\u56db\u5143\u6570\uff08roll, pitch, yaw\uff09\u3002\u56db\u5143\u7d20\u4e0d\u76f4\u89c2\uff0c\u4f46\u662fros2\u7684\u6807\u51c6\u65b9\u5f0f\n static_transformStamped.transform.rotation.x = quat[0] # \u8bbe\u7f6e\u5750\u6807\u53d8\u6362\u4e2d\u7684X\u3001Y\u3001Z\u5411\u7684\u65cb\u8f6c\uff08\u56db\u5143\u6570\uff09\n static_transformStamped.transform.rotation.y = quat[1]\n static_transformStamped.transform.rotation.z = quat[2]\n static_transformStamped.transform.rotation.w = quat[3]\n\n self.tf_broadcaster.sendTransform(static_transformStamped) # \u5e7f\u64ad\u9759\u6001\u5750\u6807\u53d8\u6362\uff0c\u5e7f\u64ad\uff08\u544a\u8bc9\u6240\u6709\u8282\u70b9\uff09\u540e\u4e24\u4e2a\u5750\u6807\u7cfb\u7684\u4f4d\u7f6e\u5173\u7cfb\u4fdd\u6301\u4e0d\u53d8\n\ndef main(args=None):\n rclpy.init(args=args) # ROS2 Python\u63a5\u53e3\u521d\u59cb\u5316\n node = StaticTFBroadcaster(\"static_tf_broadcaster\") # \u521b\u5efaROS2\u8282\u70b9\u5bf9\u8c61\u5e76\u8fdb\u884c\u521d\u59cb\u5316\n rclpy.spin(node) # \u5faa\u73af\u7b49\u5f85ROS2\u9000\u51fa\n node.destroy_node() # \u9500\u6bc1\u8282\u70b9\u5bf9\u8c61\n rclpy.shutdown()\n<\/code><\/pre>\n\n\n\nself.tf_broadcaster.sendTransform(static_transformStamped) \u5c06\u5750\u6807\u7cfb\u76f8\u5bf9\u4e8e\u53e6\u4e00\u4e2a\u56fa\u5b9a\u5750\u6807\u7cfb\u7684\u7a7a\u95f4\u5173\u7cfb\uff08\u4f4d\u7f6e\u548c\u671d\u5411\uff09\u53d1\u5e03\u5230 ROS \u7cfb\u7edf\u4e2d\u3002<\/summary>\n\u6570\u636e\u5bf9\u9f50\u4e0e\u878d\u5408<\/strong>\uff1a\u5f53\u673a\u5668\u4eba\u4e0a\u6709\u6fc0\u5149\u96f7\u8fbe\u3001\u6444\u50cf\u5934\u3001IMU \u7b49\u591a\u4e2a\u4f20\u611f\u5668\u65f6\uff0c\u5b83\u4eec\u5404\u81ea\u6d4b\u91cf\u5f97\u5230\u7684\u6570\u636e\u9ed8\u8ba4\u90fd\u5728\u81ea\u8eab\u5750\u6807\u7cfb<\/strong>\u4e0b\u3002\u901a\u8fc7\u5e7f\u64ad\u8fd9\u4e9b\u4f20\u611f\u5668\u5230\u673a\u5668\u4eba\u4e2d\u5fc3\uff08\u5982\u00a0base_link<\/code>\uff09\u7684\u9759\u6001\u53d8\u6362\uff0c\u5176\u4ed6\u8282\u70b9\u5c31\u80fd\u628a\u6240\u6709\u4f20\u611f\u5668\u7684\u6570\u636e\u8f6c\u6362\u5230\u540c\u4e00\u4e2a\u5750\u6807\u7cfb\u4e0b\u8fdb\u884c\u878d\u5408\u5904\u7406\u3002<\/p>\n\n\n\n\u5b9e\u73b0\u201c\u67e5\u8be2\u5f0f\u201d\u5750\u6807\u8f6c\u6362<\/strong>\uff1a\u5176\u4ed6\u4efb\u4f55 ROS \u8282\u70b9\uff08\u5982\u5bfc\u822a\u6a21\u5757\u3001\u5b9a\u4f4d\u6a21\u5757\uff09\u90fd\u53ef\u4ee5\u901a\u8fc7\u00a0tf2<\/code>\u00a0\u63a5\u53e3\uff08\u5982\u00a0lookupTransform<\/code>\uff09\u968f\u65f6\u67e5\u8be2\u8fd9\u4e24\u4e2a\u5750\u6807\u7cfb\u4e4b\u95f4\u7684\u5173\u7cfb\u3002\u4f8b\u5982\uff0c\u77e5\u9053\u6fc0\u5149\u96f7\u8fbe\u70b9\u4e91\u4e2d\u7684\u67d0\u4e2a\u70b9\uff0c\u5728\u673a\u5668\u4eba\u5750\u6807\u7cfb\u4e0b\u662f\u4ec0\u4e48\u4f4d\u7f6e\u3002<\/p>\n\n\n\n\u53ef\u89c6\u5316\u4e0e\u8c03\u8bd5<\/strong>\uff1aRViz \u7b49\u53ef\u89c6\u5316\u5de5\u5177\u4f1a\u76d1\u542c TF \u5e7f\u64ad\u3002\u5e7f\u64ad\u540e\uff0c\u4f60\u5c31\u80fd\u5728 RViz \u4e2d\u770b\u5230\u5750\u6807\u8f74\u7684\u56fe\u5f62\u663e\u793a\uff0c\u76f4\u89c2\u68c0\u67e5\u5404\u90e8\u4ef6\u7684\u4f4d\u7f6e\u662f\u5426\u5b89\u88c5\u6b63\u786e\uff0c\u6216\u89c2\u5bdf\u673a\u5668\u4eba\u7684\u8fd0\u52a8\u8f68\u8ff9\u3002<\/p>\n\n\n\n\u9759\u6001\u4e0e\u52a8\u6001\u53d8\u6362<\/strong>\uff1a\u6b64\u5904static_transformStamped<\/code>\u00a0\u901a\u5e38\u8868\u793a\u9759\u6001\u53d8\u6362<\/strong>\uff08\u5982\u4f20\u611f\u5668\u5728\u8f66\u8eab\u4e0a\u7684\u56fa\u5b9a\u5b89\u88c5\u4f4d\u7f6e\uff09\u3002\u8fd9\u79cd\u5173\u7cfb\u4e0d\u53d8\uff0c\u53ea\u9700\u5e7f\u64ad\u4e00\u6b21\uff0c\u540e\u7eed\u4f1a\u88ab\u7f13\u5b58\u5e76\u6c38\u4e45\u4f7f\u7528\u3002\u5982\u679c\u662f\u52a8\u6001\u53d8\u6362\uff08\u5982\u968f\u5173\u8282\u8f6c\u52a8\u7684\u673a\u68b0\u81c2\u672b\u7aef\uff09\uff0c\u5219\u9700\u8981\u4ee5\u9ad8\u9891\uff08\u5982 100Hz\uff09\u6301\u7eed\u5e7f\u64ad\uff0c\u4ee5\u53cd\u6620\u5b9e\u65f6\u8fd0\u52a8\u3002<\/p>\n<\/details>\n\n\n\n\u53ef\u4ee5\u5b9a\u65f6\u76d1\u542c\u4e24\u4e2a\u5750\u6807\u7cfb\u7684\u60c5\u51b5\uff1a<\/p>\n\n\n\n
import rclpy # ROS2 Python\u63a5\u53e3\u5e93\nfrom rclpy.node import Node # ROS2 \u8282\u70b9\u7c7b\nimport tf_transformations # TF\u5750\u6807\u53d8\u6362\u5e93\nfrom tf2_ros import TransformException # TF\u5de6\u8fb9\u53d8\u6362\u7684\u5f02\u5e38\u7c7b\nfrom tf2_ros.buffer import Buffer # \u5b58\u50a8\u5750\u6807\u53d8\u6362\u4fe1\u606f\u7684\u7f13\u51b2\u7c7b\nfrom tf2_ros.transform_listener import TransformListener # \u76d1\u542c\u5750\u6807\u53d8\u6362\u7684\u76d1\u542c\u5668\u7c7b\n\nclass TFListener(Node):\n\n def __init__(self, name):\n super().__init__(name) # ROS2\u8282\u70b9\u7236\u7c7b\u521d\u59cb\u5316\n\n # \u901a\u8fc7\u53c2\u6570\uff0c\u63d0\u9ad8\u4ee3\u7801\u590d\u7528\u6027\n self.declare_parameter('source_frame', 'world') # \u521b\u5efa\u4e00\u4e2a\u6e90\u5750\u6807\u7cfb\u540d\u7684\u53c2\u6570\n self.source_frame = self.get_parameter( # \u4f18\u5148\u4f7f\u7528\u5916\u90e8\u8bbe\u7f6e\u7684\u53c2\u6570\u503c\uff0c\u5426\u5219\u7528\u9ed8\u8ba4\u503c\n 'source_frame').get_parameter_value().string_value\n\n self.declare_parameter('target_frame', 'house') # \u521b\u5efa\u4e00\u4e2a\u76ee\u6807\u5750\u6807\u7cfb\u540d\u7684\u53c2\u6570\n self.target_frame = self.get_parameter( # \u4f18\u5148\u4f7f\u7528\u5916\u90e8\u8bbe\u7f6e\u7684\u53c2\u6570\u503c\uff0c\u5426\u5219\u7528\u9ed8\u8ba4\u503c\n 'target_frame').get_parameter_value().string_value\n\n self.tf_buffer = Buffer() # \u521b\u5efa\u4fdd\u5b58\u5750\u6807\u53d8\u6362\u4fe1\u606f\u7684\u7f13\u51b2\u533a\n self.tf_listener = TransformListener(self.tf_buffer, self) # \u521b\u5efa\u5750\u6807\u53d8\u6362\u7684\u76d1\u542c\u5668\n\n self.timer = self.create_timer(1.0, self.on_timer) # \u521b\u5efa\u4e00\u4e2a\u56fa\u5b9a\u5468\u671f\u7684\u5b9a\u65f6\u5668\uff0c\u5904\u7406\u5750\u6807\u4fe1\u606f\uff0c\u9891\u7387\u4e3a1hz\uff0cself.on_timer\u4e3a\u56de\u8c03\u51fd\u6570\uff0c\u4ee51hz\u9891\u7387\u8c03\u7528\u5b83\n\n def on_timer(self):\n try:\n now = rclpy.time.Time() # \u83b7\u53d6ROS\u7cfb\u7edf\u7684\u5f53\u524d\u65f6\u95f4\n trans = self.tf_buffer.lookup_transform( # \u76d1\u542c\u5f53\u524d\u65f6\u523b\u6e90\u5750\u6807\u7cfb\u5230\u76ee\u6807\u5750\u6807\u7cfb\u7684\u5750\u6807\u53d8\u6362\uff0cnow\u6307\u5b9a\u4e86\u5f53\u524d\u65f6\u95f4\u3002\u8fd4\u56de\u7684\u662f\u4e00\u4e2a\u5750\u6807\u53d8\u6362\u6570\u503c\uff0c\u548c\u4e0a\u4e00\u4e2a\u6587\u4ef6\u4e2d\u7684tf_broadcaster \u7ed3\u6784\u76f8\u540c\n self.target_frame,\n self.source_frame,\n now)\n except TransformException as ex: # \u5982\u679c\u5750\u6807\u53d8\u6362\u83b7\u53d6\u5931\u8d25\uff0c\u8fdb\u5165\u5f02\u5e38\u62a5\u544a\n self.get_logger().info(\n f'Could not transform {self.target_frame} to {self.source_frame}: {ex}')\n return\n \n pos = trans.transform.translation # \u83b7\u53d6\u4f4d\u7f6e\u4fe1\u606f\uff08\u5e73\u79fb\u53d8\u6362\uff09\n quat = trans.transform.rotation # \u83b7\u53d6\u59ff\u6001\u4fe1\u606f\uff08\u56db\u5143\u6570\uff09\n euler = tf_transformations.euler_from_quaternion([quat.x, quat.y, quat.z, quat.w]) # \u56db\u5143\u7d20\u8f6c\u53d8\u4e3a\u6b27\u62c9\u89d2\uff0c\u65b9\u4fbf\u89c2\u5bdf\n self.get_logger().info('Get %s --> %s transform: [%f, %f, %f] [%f, %f, %f]' \n % (self.source_frame, self.target_frame, pos.x, pos.y, pos.z, euler[0], euler[1], euler[2]))\n\ndef main(args=None):\n rclpy.init(args=args) # ROS2 Python\u63a5\u53e3\u521d\u59cb\u5316\n node = TFListener(\"tf_listener\") # \u521b\u5efaROS2\u8282\u70b9\u5bf9\u8c61\u5e76\u8fdb\u884c\u521d\u59cb\u5316\n rclpy.spin(node) # \u5faa\u73af\u7b49\u5f85ROS2\u9000\u51fa\n node.destroy_node() # \u9500\u6bc1\u8282\u70b9\u5bf9\u8c61\n rclpy.shutdown() # \u5173\u95edROS2 Python\u63a5\u53e3\n<\/code><\/pre>\n\n\n\n<\/p>\n\n\n\n
<\/p>\n\n\n\n
\u81ea\u884c\u624b\u52a8\u590d\u73b0\u4e0a\u9762\u6d77\u9f9f\u8ddf\u968f\u7684\u4f8b\u5b50\uff1a<\/p>\n\n\n\n
launch\u6587\u4ef6\u5982\u4e0b\uff1a<\/p>\n\n\n\n
from launch import LaunchDescription\nfrom launch.actions import DeclareLaunchArgument\nfrom launch.substitutions import LaunchConfiguration\nfrom launch_ros.actions import Node\n\n\ndef generate_launch_description():\n return LaunchDescription([\n Node(\n package='turtlesim',\n executable='turtlesim_node',\n name='sim'\n ),\n Node(\n package='learning_tf',\n executable='turtle_tf_broadcaster',\n name='broadcaster1',\n parameters=[\n {'turtlename': 'turtle1'} # \u8f93\u5165\u53c2\u6570\u3002\u4e24\u4e2a\u8282\u70b9\u90fd\u5728\u5e7f\u64ad\u6d77\u9f9f\u76f8\u5bf9world\u5750\u6807\u7cfb\u7684\u53d8\u5316\uff0c\u4e0e\u4e0b\u9762\u90a3\u4e2anode\u51e0\u4e4e\u4e00\u6837\uff0c\u4f46\u901a\u8fc7\u53c2\u6570\u4fee\u6539\u5b9e\u73b0\u7684\u8d44\u6e90\u91cd\u5b9a\u5411\n ]\n ),\n DeclareLaunchArgument(\n 'target_frame', default_value='turtle1',\n description='Target frame name.'\n ),\n Node(\n package='learning_tf',\n executable='turtle_tf_broadcaster', # \u4e0e\u4e0a\u4e00\u4e2anode\u7684\u53ef\u6267\u884c\u6587\u4ef6\u76f8\u540c\n name='broadcaster2', # \u3002\u4e0e\u4e0a\u4e00\u4e2anode\u529f\u80fd\u5305\u4e0e\u547d\u4ee4\u76f8\u540c\uff0c\u4f46\u540d\u5b57\u4e0d\u540c\uff0c\u9632\u6b62\u91cd\u540d\n parameters=[\n {'turtlename': 'turtle2'}\n ]\n ),\n Node(\n package='learning_tf',\n executable='turtle_following', # \u76d1\u542c\u4e24\u4e2a\u6d77\u9f9f\u4f4d\u7f6e\u5173\u7cfb\u7684\u53ef\u6267\u884c\u6587\u4ef6\n name='listener',\n parameters=[\n {'target_frame': LaunchConfiguration('target_frame')}\n ]\n ), \n ])<\/code><\/pre>\n\n\n\n\u6b64\u5904DeclareLaunchArgument<\/code>\u00a0\u7684\u4f5c\u7528\u662f\u58f0\u660e\u4e00\u4e2a\u53ef\u7531\u7528\u6237\u5728\u542f\u52a8\u65f6\u81ea\u5b9a\u4e49\u7684 launch \u53c2\u6570<\/strong>\u3002\u5728\u8fd9\u4e2a\u4f8b\u5b50\u4e2d\uff0c\u5b83\u58f0\u660e\u4e86\u540d\u4e3a\u00a0target_frame<\/code>\u00a0\u7684\u53c2\u6570\uff0c\u9ed8\u8ba4\u503c\u4e3a\u00a0'turtle1'<\/code>\uff0c\u63cf\u8ff0\u4e3a\u201c\u76ee\u6807\u5750\u6807\u7cfb\u540d\u79f0\u201d\u3002\u901a\u8fc7\u8fd9\u4e2a\u58f0\u660e\uff0c\u7528\u6237\u53ef\u4ee5\u5728\u8fd0\u884c launch \u6587\u4ef6\u65f6\u901a\u8fc7\u547d\u4ee4\u884c\u4f20\u9012\u4e0d\u540c\u7684\u503c\uff08\u4f8b\u5982\u00a0target_frame:=turtle2<\/code>\uff09\uff0c\u8be5\u503c\u968f\u540e\u4f1a\u901a\u8fc7\u00a0LaunchConfiguration('target_frame')<\/code>\u00a0\u4f20\u9012\u7ed9\u4e0b\u6e38\u8282\u70b9\uff08\u8fd9\u91cc\u662f\u00a0turtle_following<\/code>\u00a0\u8282\u70b9\u7684\u00a0target_frame<\/code>\u00a0\u53c2\u6570\uff09\uff0c\u4ece\u800c\u7075\u6d3b\u6539\u53d8\u8282\u70b9\u7684\u884c\u4e3a\uff0c\u800c\u65e0\u9700\u4fee\u6539 launch \u6587\u4ef6\u672c\u8eab\u3002\u7b80\u8a00\u4e4b\uff0c\u5b83\u8ba9 launch \u6587\u4ef6\u53d8\u5f97\u66f4\u52a0\u901a\u7528\u548c\u53ef\u914d\u7f6e\u3002<\/p>\n\n\n\n\u5176\u4e2dturtle_tf_broadcaster\u53ef\u6267\u884c\u6587\u4ef6\u7684\u4ee3\u7801\u5982\u4e0b\uff1a<\/p>\n\n\n\n
import rclpy # ROS2 Python\u63a5\u53e3\u5e93\nfrom rclpy.node import Node # ROS2 \u8282\u70b9\u7c7b\nfrom geometry_msgs.msg import TransformStamped # \u5750\u6807\u53d8\u6362\u6d88\u606f\nimport tf_transformations # TF\u5750\u6807\u53d8\u6362\u5e93\nfrom tf2_ros import TransformBroadcaster # TF\u5750\u6807\u53d8\u6362\u5e7f\u64ad\u5668\nfrom turtlesim.msg import Pose # turtlesim\u5c0f\u6d77\u9f9f\u4f4d\u7f6e\u6d88\u606f\n\nclass TurtleTFBroadcaster(Node):\n\n def __init__(self, name):\n super().__init__(name) # ROS2\u8282\u70b9\u7236\u7c7b\u521d\u59cb\u5316\n\n self.declare_parameter('turtlename', 'turtle') # \u521b\u5efa\u4e00\u4e2a\u6d77\u9f9f\u540d\u79f0\u7684\u53c2\u6570\uff0c\u901a\u8fc7launch\u8bbe\u7f6e\u7684\u53c2\u6570\u53d6\u4ee3\u6d77\u9f9f\u7684\u540d\u5b57\n self.turtlename = self.get_parameter( # \u4f18\u5148\u4f7f\u7528\u5916\u90e8\u8bbe\u7f6e\u7684\u53c2\u6570\u503c\uff0c\u5426\u5219\u7528\u9ed8\u8ba4\u503c\n 'turtlename').get_parameter_value().string_value\n\n self.tf_broadcaster = TransformBroadcaster(self) # \u521b\u5efa\u4e00\u4e2aTF\u5750\u6807\u53d8\u6362\u7684\u5e7f\u64ad\u5bf9\u8c61\u5e76\u521d\u59cb\u5316\uff0c\u4fbf\u4e8e\u540e\u7eed\u5e7f\u64ad\u6d77\u9f9f\u5730\u5740\n\n self.subscription = self.create_subscription( # \u521b\u5efa\u4e00\u4e2a\u8ba2\u9605\u8005\uff0c\u8ba2\u9605\u6d77\u9f9f\u7684\u4f4d\u7f6e\u6d88\u606f\n Pose,\n f'\/{self.turtlename}\/pose', # \u4f7f\u7528\u53c2\u6570\u4e2d\u83b7\u53d6\u5230\u7684\u6d77\u9f9f\u540d\u79f0\n self.turtle_pose_callback, 1)\n\n def turtle_pose_callback(self, msg): # \u521b\u5efa\u4e00\u4e2a\u5904\u7406\u6d77\u9f9f\u4f4d\u7f6e\u6d88\u606f\u7684\u56de\u8c03\u51fd\u6570\uff0c\u5c06\u4f4d\u7f6e\u6d88\u606f\u8f6c\u53d8\u6210\u5750\u6807\u53d8\u6362\uff0cmsg\u5373\u8ba2\u9605\u5230\u7684\u6d88\u606f\n transform = TransformStamped() # \u521b\u5efa\u4e00\u4e2a\u5750\u6807\u53d8\u6362\u7684\u6d88\u606f\u5bf9\u8c61\n\n transform.header.stamp = self.get_clock().now().to_msg() # \u8bbe\u7f6e\u5750\u6807\u53d8\u6362\u6d88\u606f\u7684\u65f6\u95f4\u6233\n transform.header.frame_id = 'world' # \u8bbe\u7f6e\u4e00\u4e2a\u5750\u6807\u53d8\u6362\u7684\u6e90\u5750\u6807\u7cfb\n transform.child_frame_id = self.turtlename # \u8bbe\u7f6e\u4e00\u4e2a\u5750\u6807\u53d8\u6362\u7684\u76ee\u6807\u5750\u6807\u7cfb\n transform.transform.translation.x = msg.x # \u8bbe\u7f6e\u5750\u6807\u53d8\u6362\u4e2d\u7684X\u3001Y\u3001Z\u5411\u7684\u5e73\u79fb\n transform.transform.translation.y = msg.y\n transform.transform.translation.z = 0.0\n q = tf_transformations.quaternion_from_euler(0, 0, msg.theta) # \u5c06\u6b27\u62c9\u89d2\u8f6c\u6362\u4e3a\u56db\u5143\u6570\uff08roll, pitch, yaw\uff09\n transform.transform.rotation.x = q[0] # \u8bbe\u7f6e\u5750\u6807\u53d8\u6362\u4e2d\u7684X\u3001Y\u3001Z\u5411\u7684\u65cb\u8f6c\uff08\u56db\u5143\u6570\uff09\n transform.transform.rotation.y = q[1]\n transform.transform.rotation.z = q[2]\n transform.transform.rotation.w = q[3]\n\n # Send the transformation\n self.tf_broadcaster.sendTransform(transform) # \u5e7f\u64ad\u5750\u6807\u53d8\u6362\uff0c\u6d77\u9f9f\u4f4d\u7f6e\u53d8\u5316\u540e\uff0c\u5c06\u53ca\u65f6\u66f4\u65b0\u5750\u6807\u53d8\u6362\u4fe1\u606f\uff0c\u52a8\u6001\u7ef4\u62a4\u4e24\u5750\u6807\u7cfb\u4f4d\u7f6e\u7684\u76f8\u5bf9\u5173\u7cfb\n\ndef main(args=None):\n rclpy.init(args=args) # ROS2 Python\u63a5\u53e3\u521d\u59cb\u5316\n node = TurtleTFBroadcaster(\"turtle_tf_broadcaster\") # \u521b\u5efaROS2\u8282\u70b9\u5bf9\u8c61\u5e76\u8fdb\u884c\u521d\u59cb\u5316\n rclpy.spin(node) # \u5faa\u73af\u7b49\u5f85ROS2\u9000\u51fa\n node.destroy_node() # \u9500\u6bc1\u8282\u70b9\u5bf9\u8c61\n rclpy.shutdown() # \u5173\u95edROS2 Python\u63a5\u53e3\n<\/code><\/pre>\n\n\n\nturtle_following\u7684\u53ef\u6267\u884c\u6587\u4ef6\u4ee3\u7801\u5982\u4e0b\uff1a<\/p>\n\n\n\n
import math\nimport rclpy # ROS2 Python\u63a5\u53e3\u5e93\nfrom rclpy.node import Node # ROS2 \u8282\u70b9\u7c7b\nimport tf_transformations # TF\u5750\u6807\u53d8\u6362\u5e93\nfrom tf2_ros import TransformException # TF\u5de6\u8fb9\u53d8\u6362\u7684\u5f02\u5e38\u7c7b\nfrom tf2_ros.buffer import Buffer # \u5b58\u50a8\u5750\u6807\u53d8\u6362\u4fe1\u606f\u7684\u7f13\u51b2\u7c7b\nfrom tf2_ros.transform_listener import TransformListener # \u76d1\u542c\u5750\u6807\u53d8\u6362\u7684\u76d1\u542c\u5668\u7c7b\nfrom geometry_msgs.msg import Twist # ROS2 \u901f\u5ea6\u63a7\u5236\u6d88\u606f\nfrom turtlesim.srv import Spawn # \u6d77\u9f9f\u751f\u6210\u7684\u670d\u52a1\u63a5\u53e3\nclass TurtleFollowing(Node):\n\n def __init__(self, name):\n super().__init__(name) # ROS2\u8282\u70b9\u7236\u7c7b\u521d\u59cb\u5316\n\n self.declare_parameter('source_frame', 'turtle1') # \u521b\u5efa\u4e00\u4e2a\u6e90\u5750\u6807\u7cfb\u540d\u7684\u53c2\u6570\n self.source_frame = self.get_parameter( # \u4f18\u5148\u4f7f\u7528\u5916\u90e8\u8bbe\u7f6e\u7684\u53c2\u6570\u503c\uff0c\u5426\u5219\u7528\u9ed8\u8ba4\u503c\n 'source_frame').get_parameter_value().string_value\n\n self.tf_buffer = Buffer() # \u521b\u5efa\u4fdd\u5b58\u5750\u6807\u53d8\u6362\u4fe1\u606f\u7684\u7f13\u51b2\u533a\n self.tf_listener = TransformListener(self.tf_buffer, self) # \u521b\u5efa\u5750\u6807\u53d8\u6362\u7684\u76d1\u542c\u5668\n\n self.spawner = self.create_client(Spawn, 'spawn') # \u521b\u5efa\u4e00\u4e2a\u8bf7\u6c42\u4ea7\u751f\u6d77\u9f9f\u7684\u5ba2\u6237\u7aef\uff0c\u7528\u4e8e\u521b\u5efa\u7b2c\u4e8c\u53ea\u6d77\u9f9f\n self.turtle_spawning_service_ready = False # \u662f\u5426\u5df2\u7ecf\u8bf7\u6c42\u6d77\u9f9f\u751f\u6210\u670d\u52a1\u7684\u6807\u5fd7\u4f4d\n self.turtle_spawned = False # \u6d77\u9f9f\u662f\u5426\u4ea7\u751f\u6210\u529f\u7684\u6807\u5fd7\u4f4d\n\n self.publisher = self.create_publisher(Twist, 'turtle2\/cmd_vel', 1) # \u521b\u5efa\u8ddf\u968f\u8fd0\u52a8\u6d77\u9f9f\u7684\u901f\u5ea6\u8bdd\u9898\n\n self.timer = self.create_timer(1.0, self.on_timer) # \u521b\u5efa\u4e00\u4e2a\u56fa\u5b9a\u5468\u671f\u7684\u5b9a\u65f6\u5668\uff0c\u63a7\u5236\u8ddf\u968f\u6d77\u9f9f\u7684\u8fd0\u52a8\n\n def on_timer(self):\n from_frame_rel = self.source_frame # \u6e90\u5750\u6807\u7cfb\n to_frame_rel = 'turtle2' # \u76ee\u6807\u5750\u6807\u7cfb\n\n if self.turtle_spawning_service_ready: # \u5982\u679c\u5df2\u7ecf\u8bf7\u6c42\u6d77\u9f9f\u751f\u6210\u670d\u52a1\n if self.turtle_spawned: # \u5982\u679c\u8ddf\u968f\u6d77\u9f9f\u5df2\u7ecf\u751f\u6210\n try:\n now = rclpy.time.Time() # \u83b7\u53d6ROS\u7cfb\u7edf\u7684\u5f53\u524d\u65f6\u95f4\n trans = self.tf_buffer.lookup_transform( # \u76d1\u542c\u5f53\u524d\u65f6\u523b\u6e90\u5750\u6807\u7cfb\u5230\u76ee\u6807\u5750\u6807\u7cfb\u7684\u5750\u6807\u53d8\u6362\n to_frame_rel,\n from_frame_rel,\n now)\n except TransformException as ex: # \u5982\u679c\u5750\u6807\u53d8\u6362\u83b7\u53d6\u5931\u8d25\uff0c\u8fdb\u5165\u5f02\u5e38\u62a5\u544a\n self.get_logger().info(\n f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}')\n return\n\n msg = Twist() # \u521b\u5efa\u901f\u5ea6\u63a7\u5236\u6d88\u606f\n scale_rotation_rate = 1.0 # \u6839\u636e\u6d77\u9f9f\u89d2\u5ea6\uff0c\u8ba1\u7b97\u89d2\u901f\u5ea6\n msg.angular.z = scale_rotation_rate * math.atan2(\n trans.transform.translation.y,\n trans.transform.translation.x)\n\n scale_forward_speed = 0.5 # \u6839\u636e\u6d77\u9f9f\u8ddd\u79bb\uff0c\u8ba1\u7b97\u7ebf\u901f\u5ea6\n msg.linear.x = scale_forward_speed * math.sqrt(\n trans.transform.translation.x ** 2 +\n trans.transform.translation.y ** 2)\n\n self.publisher.publish(msg) # \u53d1\u5e03\u901f\u5ea6\u6307\u4ee4\uff0c\u6d77\u9f9f\u8ddf\u968f\u8fd0\u52a8\n else: # \u5982\u679c\u8ddf\u968f\u6d77\u9f9f\u6ca1\u6709\u751f\u6210\n if self.result.done(): # \u67e5\u770b\u6d77\u9f9f\u662f\u5426\u751f\u6210\n self.get_logger().info(\n f'Successfully spawned {self.result.result().name}')\n self.turtle_spawned = True \n else: # \u4f9d\u7136\u6ca1\u6709\u751f\u6210\u8ddf\u968f\u6d77\u9f9f\n self.get_logger().info('Spawn is not finished')\n else: # \u5982\u679c\u6ca1\u6709\u8bf7\u6c42\u6d77\u9f9f\u751f\u6210\u670d\u52a1\n if self.spawner.service_is_ready(): # \u5982\u679c\u6d77\u9f9f\u751f\u6210\u670d\u52a1\u5668\u5df2\u7ecf\u51c6\u5907\u5c31\u7eea\n request = Spawn.Request() # \u521b\u5efa\u4e00\u4e2a\u8bf7\u6c42\u7684\u6570\u636e\n request.name = 'turtle2' # \u8bbe\u7f6e\u8bf7\u6c42\u6570\u636e\u7684\u5185\u5bb9\uff0c\u5305\u62ec\u6d77\u9f9f\u540d\u3001xy\u4f4d\u7f6e\u3001\u59ff\u6001\n request.x = float(4)\n request.y = float(2)\n request.theta = float(0)\n\n self.result = self.spawner.call_async(request) # \u53d1\u9001\u670d\u52a1\u8bf7\u6c42\n self.turtle_spawning_service_ready = True # \u8bbe\u7f6e\u6807\u5fd7\u4f4d\uff0c\u8868\u793a\u5df2\u7ecf\u53d1\u9001\u8bf7\u6c42\n else:\n self.get_logger().info('Service is not ready') # \u6d77\u9f9f\u751f\u6210\u670d\u52a1\u5668\u8fd8\u6ca1\u51c6\u5907\u5c31\u7eea\u7684\u63d0\u793a\n\n\ndef main(args=None):\n rclpy.init(args=args) # ROS2 Python\u63a5\u53e3\u521d\u59cb\u5316\n node = TurtleFollowing(\"turtle_following\") # \u521b\u5efaROS2\u8282\u70b9\u5bf9\u8c61\u5e76\u8fdb\u884c\u521d\u59cb\u5316\n rclpy.spin(node) # \u5faa\u73af\u7b49\u5f85ROS2\u9000\u51fa\n node.destroy_node() # \u9500\u6bc1\u8282\u70b9\u5bf9\u8c61\n rclpy.shutdown() # \u5173\u95edROS2 Python\u63a5\u53e3<\/code><\/pre>\n","protected":false},"excerpt":{"rendered":"\u5750\u6807\u7cfb\u662f\u6211\u4eec\u975e\u5e38\u719f\u6089\u7684\u4e00\u4e2a\u6982\u5ff5\uff0c\u4e5f\u662f\u673a\u5668\u4eba\u5b66\u4e2d\u7684\u91cd\u8981\u57fa\u7840\uff0c\u5728\u4e00\u4e2a\u5b8c\u6574\u7684\u673a\u5668\u4eba\u7cfb\u7edf\u4e2d\uff0c\u4f1a\u5b58\u5728\u5f88\u591a\u5750\u6807\u7cfb\u3002 \u5728\u673a\u68b0 […]<\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[9],"tags":[],"class_list":["post-164","post","type-post","status-publish","format-standard","hentry","category-ros2"],"_links":{"self":[{"href":"https:\/\/374969782.xyz\/index.php\/wp-json\/wp\/v2\/posts\/164","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/374969782.xyz\/index.php\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/374969782.xyz\/index.php\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/374969782.xyz\/index.php\/wp-json\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/374969782.xyz\/index.php\/wp-json\/wp\/v2\/comments?post=164"}],"version-history":[{"count":2,"href":"https:\/\/374969782.xyz\/index.php\/wp-json\/wp\/v2\/posts\/164\/revisions"}],"predecessor-version":[{"id":166,"href":"https:\/\/374969782.xyz\/index.php\/wp-json\/wp\/v2\/posts\/164\/revisions\/166"}],"wp:attachment":[{"href":"https:\/\/374969782.xyz\/index.php\/wp-json\/wp\/v2\/media?parent=164"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/374969782.xyz\/index.php\/wp-json\/wp\/v2\/categories?post=164"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/374969782.xyz\/index.php\/wp-json\/wp\/v2\/tags?post=164"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}