ROS Resources: Documentation | Support | Discussion Forum | Service Status | Q&A answers.ros.org
Ask Your Question
0

Publisher publish once and stops publishing

asked 2013-05-09 23:28:47 -0500

whiterose gravatar image

updated 2014-01-28 17:16:29 -0500

ngrennan gravatar image

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)

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
2

answered 2013-05-10 06:05:17 -0500

As @Souldier said, ros, will block on the rospy.spin() call until ROS is shutdown. As described here, this is mostly used to prevent your code from exiting until ROS is shutdown. An equivalent method is to use a loop:

while not rospy.is_shutdown():
  rospy.sleep(1.0)

You already have the loop, so don't need the call to rospy.spin(). Just remove that call, and you should be fine.

In C++ applications (using roscpp), you need to call either ros::spin() or ros::spinOnce() to ensure that ROS messages and callbacks are processed properly. In python, this is not required. The rospy.spin() call is just used to prevent your application from exiting early.

For more discussion on this subject, see here and here.

edit flag offensive delete link more

Comments

+1 for more detail

Souldier gravatar imageSouldier ( 2013-05-10 06:26:58 -0500 )edit
0

answered 2013-05-10 02:47:30 -0500

Souldier gravatar image

It may be because you are calling rospy.spin() in your Main loop. According to the documentation on rospy.spin() (see here and search for "spin") it "Blocks until ROS node is shutdown." - which could explain why you are only seeing the first pass through the talker method..

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2013-05-09 23:28:47 -0500

Seen: 1,806 times

Last updated: May 10 '13