Publisher publish once and stops publishing

edit

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 ... (more)