Publisher publish once and stops publishing
Hi all,
I have the following code. It seems that I am able to publish the topics only once at start of node, after that the topic is not published anymore. Any ideas?
#!/usr/bin/env python
import roslib; roslib.load_manifest('brsu_hmm_eid_observations')
import rospy
import collections
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from brsu_hmm_eid_messages.msg import obs_base_x_vel_diff, obs_base_x_vel_cmd, obs_base_x_acc, obs_base_x_jerk # all contains float64 and Header.
class get_obs_base_x_all():
def __init__(self):
self.duration_acc = 4
self.duration_jerk = 8
self.base_x_vel = collections.deque(maxlen = self.duration_jerk)
self.base_x_acc = collections.deque(maxlen = self.duration_jerk)
for i in xrange(self.duration_acc):
self.base_x_vel.append(0)
for i in xrange(self.duration_jerk):
self.base_x_acc.append(0)
self.base_x_vel_cmd = 0
self.base_x_vel_diff = 0
self.base_x_jerk = 0
def listener(self):
rospy.Subscriber("odom", Odometry, self.odom_callback)
rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback)
#self.talker()
def odom_callback(self, msg):
t = rospy.Time.now().secs
self.base_x_vel.append(msg.twist.twist.linear.x)
self.calc_base_x_acc(t)
self.calc_base_x_jerk(t)
def cmd_vel_callback(self, msg):
if msg.linear.x is None:
self.base_x_vel_cmd = 0
else:
self.base_x_vel_cmd = msg.linear.x
self.calc_base_x_vel_diff()
def calc_base_x_vel_diff(self):
self.base_x_vel_diff = self.base_x_vel_cmd - self.base_x_vel[self.duration_acc - 1]
#print 'vel_diff', self.base_x_vel_diff
def calc_base_x_acc(self, t):
diff = int(t) % self.duration_acc
if int(t) < self.duration_acc:
self.base_x_acc = 0
elif diff == 0 :
acc_x = (self.base_x_vel[self.duration_acc - 1] - self.base_x_vel[0])/self.duration_acc
#print 'acc_x', acc_x
self.base_x_acc.append(acc_x)
#print self.base_x_acc[self.duration_jerk - 1]
def calc_base_x_jerk(self, t):
diff = int(t) % self.duration_jerk
if int(t) < self.duration_jerk:
self.base_x_jerk = 0
elif diff == 0 :
#jerk_x = (self.base_x_acc[self.duration_jerk - 1] - self.base_x_acc[0])/self.duration_jerk
jerk_x = ((self.base_x_vel[self.duration_jerk - 1] - self.base_x_vel[0])/self.duration_jerk)/self.duration_jerk
#print 'jerk_x',jerk_x
self.base_x_jerk = jerk_x
#print self.base_x_jerk
def talker(self):
rospy.loginfo("Publishing obs_base_x_all")
time_now = rospy.Time.now().secs
pub1 = rospy.Publisher('/brsu_hmm_eid_observations/base_x_vel_cmd', obs_base_x_vel_cmd)#, latch=True)
pub2 = rospy.Publisher('/brsu_hmm_eid_observations/base_x_vel_diff', obs_base_x_vel_diff)
pub3 = rospy.Publisher('/brsu_hmm_eid_observations/base_x_acc', obs_base_x_acc)
pub4 = rospy.Publisher('/brsu_hmm_eid_observations/base_x_jerk', obs_base_x_jerk)
msg2send1 = obs_base_x_vel_cmd()
msg2send1.header.stamp.secs = rospy.Time.now().secs
msg2send1.x_cmd_vel = self.base_x_vel_cmd
pub1.publish(msg2send1)
print 'self.base_x_vel_cmd', self.base_x_vel_cmd
msg2send2 = obs_base_x_vel_diff()
msg2send2.header.stamp.secs = rospy.Time.now().secs
msg2send2.x_vel_diff = self.base_x_vel_diff
pub2.publish(msg2send2)
print 'self.base_x_vel_dif', self.base_x_vel_diff
msg2send3 = obs_base_x_acc()
msg2send3.header.stamp.secs = time_now
msg2send3.x_acc = self.base_x_acc[self.duration_jerk - 1]
pub3.publish(msg2send3)
print 'self.base_x_acc[self.duration_jerk - 1]', self.base_x_acc[self.duration_jerk - 1]
msg2send4 = obs_base_x_jerk()
msg2send4.header.stamp.secs = time_now
msg2send4.x_jerk = self.base_x_jerk
pub4.publish(msg2send4)
print 'self.base_x_jerk', self.base_x_jerk
rospy.sleep(1)
rospy.loginfo('published base x all')
if __name__ == '__main__':
rospy.init_node('brsu_hmm_eid_observations_node_x_all', anonymous=True)
gobxa = get_obs_base_x_all()
while not rospy.is_shutdown():
try:
gobxa.listener()
gobxa.talker()
rospy.spin()
except rospy.ROSInterruptException:
pass
At first I thought it is due to subscribing the cmd_vel topic, where values are only available when we give some. But it happens both when the the robot is moving and not. I think I missed something very trivial ...