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")
