def __init__(self): rospy.init_node('velocity') self.velocity_pub = rospy.Publisher('/velocity', MsgVelocity, queue_size=10) self.velocity = Velocity() self.msg_velocity = MsgVelocity() self.listener = tf.TransformListener() r = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): try: self.pub_velocity() except: rospy.logerr("pub_velocity") r.sleep()
def __init__(self): rospy.init_node('velocity') self.velocity_pub = rospy.Publisher( '/velocity', MsgVelocity, queue_size=10) self.velocity = Velocity() self.msg_velocity = MsgVelocity() self.listener = tf.TransformListener() r = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): try: self.pub_velocity() except: rospy.logerr("pub_velocity") r.sleep()
class NdVelocity(): def __init__(self): rospy.init_node('velocity') self.velocity_pub = rospy.Publisher('/velocity', MsgVelocity, queue_size=10) self.velocity = Velocity() self.msg_velocity = MsgVelocity() self.listener = tf.TransformListener() r = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): try: self.pub_velocity() except: rospy.logerr("pub_velocity") r.sleep() def pub_velocity(self): # Make sure we see the world and tcp frames self.listener.waitForTransform("/world", "/tcp0", rospy.Time(), rospy.Duration(5.0)) try: stamp = rospy.Time.now() self.listener.waitForTransform("/world", "/tcp0", stamp, rospy.Duration(1.0)) position, quaternion = self.listener.lookupTransform( "/world", "/tcp0", stamp) speed, velocity = self.velocity.instantaneous( stamp.to_sec(), np.array(position)) self.msg_velocity.header.stamp = stamp self.msg_velocity.speed = speed self.msg_velocity.vx = velocity[0] self.msg_velocity.vy = velocity[1] self.msg_velocity.vz = velocity[2] rospy.loginfo(self.msg_velocity) self.velocity_pub.publish(self.msg_velocity) except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo("TF Exception")
class NdVelocity(): def __init__(self): rospy.init_node('velocity') self.velocity_pub = rospy.Publisher( '/velocity', MsgVelocity, queue_size=10) self.velocity = Velocity() self.msg_velocity = MsgVelocity() self.listener = tf.TransformListener() r = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): try: self.pub_velocity() except: rospy.logerr("pub_velocity") r.sleep() def pub_velocity(self): # Make sure we see the world and tcp frames self.listener.waitForTransform( "/world", "/tcp0", rospy.Time(), rospy.Duration(5.0)) try: stamp = rospy.Time.now() self.listener.waitForTransform( "/world", "/tcp0", stamp, rospy.Duration(1.0)) position, quaternion = self.listener.lookupTransform( "/world", "/tcp0", stamp) speed, velocity = self.velocity.instantaneous( stamp.to_sec(), np.array(position)) self.msg_velocity.header.stamp = stamp self.msg_velocity.speed = speed self.msg_velocity.vx = velocity[0] self.msg_velocity.vy = velocity[1] self.msg_velocity.vz = velocity[2] rospy.loginfo(self.msg_velocity) self.velocity_pub.publish(self.msg_velocity) except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo("TF Exception")