03 ros發布和訂閱python(如何在ros 使用odometry python)

   self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

   rate = 20

   r = rospy.Rate(rate)

   linear_speed = 0.2

   goal_distance =1.0

   angular_speed = 1.0

   goal_angle = pi

   angular_tolerance = radians(2.5)    

   # Initialize tf listener, and give some time to fill its buffer

   self.tf_listener = tf.TransformListener()

   rospy.sleep(2)    

   # Set odom_frame and base_frame

   self.odom_frame = '/odom'

   try:

     self.tf_listener.waitForTransform(self.odom_frame,

                                       '/base_footprint',

                                       rospy.Time(),

                                       rospy.Duration(1.0))

     self.base_frame = '/base_footprint'

   except(tf.Exception, tf.ConnectivityException, tf.LookupException):      try:

       self.tf_listener.waitForTransform(self.odom_frame,

                                         '/base_link',

                                         rospy.Time(),

                                         rospy.Duration(1.0))

       self.base_frame = '/base_link'

     except(tf.Exception, tf.ConnectivityException, tf.LookupException):

       rospy.loginfo("Cannot find base_frame transformed from /odom")

       rospy.signal_shutdown("tf Exception")

   position = Point()    

   for i in range(2):

     move_cmd = Twist()

     move_cmd.linear.x = linear_speed      # Initial pose, obtained from internal odometry

     (position, rotation) = self.get_odom()

     x_start = position.x

     y_start = position.y

     distance = 0      

     # Keep publishing Twist msgs, until the internal odometry reach the goal

     while distance < goal_distance and not rospy.is_shutdown():

       self.cmd_vel.publish(move_cmd)

       r.sleep()

       (position, rotation) = self.get_odom()

       distance = sqrt( pow( (position.x-x_start), 2 ) + \

         pow( (position.y-y_start), 2 ) )      

     # Stop 1 ms before rotate

     move_cmd = Twist()

     self.cmd_vel.publish(move_cmd)

     rospy.sleep(1)

     move_cmd.angular.z = angular_speed      # should be the current ration from odom

     angle_last = rotation

     angle_turn = 0      while abs(angle_turn+angular_tolerance) < abs(goal_angle) \        and not rospy.is_shutdown():

       self.cmd_vel.publish(move_cmd)

       r.sleep()

       (position, rotation) = self.get_odom

       delta_angle = normalize_angle(rotation - angle_last)

       angle_turn += delta_angle

       angle_last = rotation

     move_cmd = Twist()

     self.cmd_vel.publish(move_cmd)

     rospy.sleep(1)

     self.cmd_vel.publish(Twist())  

 def get_dom(self):    try:

     (trans, rot) = self.tf_listener.lookupTransfrom(self.odom_frame,

      &

很赞哦!（48196）