将下段代码转成python程序“#include <ros/ros.h> #include <tf/transform_listener.h> #include <geometry_msgs/Pose2D.h> #include <fstream> #include <iostream> #include <cmath> using namespace std; double x,y,z, qx,qy,qz, qw; double theta; geometry_msgs::Pose2D pos_now; int main(int argc, char** argv){ ros::init(argc, argv, "tf_Pose_Publisher"); ros::NodeHandle node; ros::Publisher _pose_pub=node.advertise<geometry_msgs::Pose2D>("pose_data", 10); tf::StampedTransform transform; tf::TransformListener listener; ros::Rate rate(10.0); while (ros::ok()){ ros::Time start = ros::Time::now(); cout << "StartTime:"<< start << endl; tf::StampedTransform transform; try{ //得到坐map和坐标base_link之间的关系 listener.waitForTransform("map","base_link", ros::Time(0), ros::Duration(3.0)); listener.lookupTransform("map", "base_link", ros::Time(0), transform); } catch (tf::TransformException &ex) { ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); } x=transform.getOrigin().x(); y=transform.getOrigin().y(); z=transform.getOrigin().z(); tf::Quaternion q = transform.getRotation(); qx = q.x(); qy = q.y(); qz = q.z(); qw = q.w(); pos_now.x = transform.getOrigin().x(); pos_now.y =transform.getOrigin().y(); pos_now.theta = tf::getYaw(q); _pose_pub.publish(pos_now); printf("x: %f, y: %f, z: %f, qx: %f,qy: %f,qz: %f, qw: %f, theta: %f\n",x,y,z,qx,qy,qz,qw,pos_now.theta); rate.sleep(); ros::Time end = ros::Time::now(); cout << "EndTime:"<<end << endl; } return 0; };

以下是将C++代码转换为Python代码的结果：

#!/usr/bin/env python

import rospy
import tf
from geometry_msgs.msg import Pose2D

x, y, z, qx, qy, qz, qw = 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
theta = 0.0
pos_now = Pose2D()

rospy.init_node('tf_Pose_Publisher')
_pose_pub = rospy.Publisher('pose_data', Pose2D, queue_size=10)
listener = tf.TransformListener()
rate = rospy.Rate(10.0)

while not rospy.is_shutdown():
    start = rospy.Time.now()
    print("StartTime: {}".format(start))
    try:
        # 得到坐map和坐标base_link之间的关系
        listener.waitForTransform("map", "base_link", rospy.Time(0), rospy.Duration(3.0))
        (trans, rot) = listener.lookupTransform("map", "base_link", rospy.Time(0))
        x = trans[0]
        y = trans[1]
        z = trans[2]
        qx = rot[0]
        qy = rot[1]
        qz = rot[2]
        qw = rot[3]
        pos_now.x = trans[0]
        pos_now.y = trans[1]
        pos_now.theta = tf.transformations.euler_from_quaternion(rot)[2]
        _pose_pub.publish(pos_now)
        print("x: {}, y: {}, z: {}, qx: {}, qy: {}, qz: {}, qw: {}, theta: {}".format(x, y, z, qx, qy, qz, qw, pos_now.theta))
    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
        rospy.sleep(1.0)
    rate.sleep()
    end = rospy.Time.now()
    print("EndTime: {}".format(end))