Ask Your Question
0

Fix the subscriber rate

asked 2017-06-23 09:32:11 -0500

Hdifsttar gravatar image

Hi everyone,

I wrote this code in order to retrieve a sonar range message, calculate a velocity based on it, and send it to my robot. I'm publishing at a 100Hz rate but subscribing at only 10Hz.(I've verified with rostopic hz). I'd like to have the same rate for both publishing and subscribing in order to have a different velocity transmitted at each loop.(And not 10 loops in a row transmitting the same information like in my example...)

How can I do that ? Is there something wrong with my code ? If anyone could help me, I'd be very grateful !

Hdifsttar

N.B. : I've read about tcpnodelay, but couldn't really understand what that does and if it fits the situation.

#########Publisher

pub_r1 = rospy.Publisher('/r1/cmd_vel', Twist)
pub_r2 = rospy.Publisher('/r2/cmd_vel', Twist)
rospy.init_node('p3dx_mover')
twist_r1 = Twist()
twist_r2 = Twist()

########Variables
i=0
rate = rospy.Rate(rospy.get_param('~hz', 100))

s_min = 0.3                         #minimum space between slave & master
s_max = 2                           #maximum space between slave & master
v_max = 0.4                         #maximum velocity of slave
alpha = v_max / (s_max - s_min)     #slope of the velocity function
b = alpha*s_max - v_max             #intercept of the velocity function

#########Subscriber

def callback(msg):
    rospy.loginfo("Sonar 4 detects something at %s m" % round(msg.range,3))
    if msg.range < 0.22:
        rospy.loginfo("Obstacle too close ! Not normal, vehicule stopped (You might want to revise the code)")
        twistr_r1.linear.x = 0
        pub_r1.publish(twist)
        rospy.signal_shutdown("Simulation end")

    beta = alpha*msg.range - b
    gamma = min(beta,v_max)
    twist_r1.linear.x = max(0, gamma)

sub = rospy.Subscriber("/r1/sonar4/scan", Range, callback)

#########Loop

while not rospy.is_shutdown() :

    twist_r2.linear.x = (math.cos(i) +2)/8
    i+=0.1
    rospy.loginfo("r1 velocity is "+ str(round(twist_r1.linear.x,3))+ " m/s" )
    rospy.loginfo("r2 velocity is "+ str(round(twist_r2.linear.x,3))+ " m/s" )
    pub_r1.publish(twist_r1)
    pub_r2.publish(twist_r2)
    rate.sleep()
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-06-23 09:41:49 -0500

Your subscriber will only call the callback method when the /r1/sonar4/scan topic is published. If that topic is only being published at 10 Hz, then you should expect that the callback will only run at 10 Hz. Your sonar sensor and corresponding driver may have settings that allow you to speed up the publish rate, but they may not. If the sensor is only capable of providing new data at 10 Hz, then it's unclear to me why you would want to run the callback more frequently than this.

edit flag offensive delete link more

Comments

That does make sense. Does it mean the sensor publish at its maximum rate by default ? Because I never set it to publish at 10Hz within my code.

edit : I found where I defined my sonar update_rate(in a xacro file of my mode, was set at 10Hz). Changing it to 100 doesn't resolve the problem though.

Hdifsttar gravatar image Hdifsttar  ( 2017-06-23 09:53:55 -0500 )edit

Is it possible the sonar cannot run faster? What sensor/driver/ROS package are you using? Have you checked the frequency of /r1/sonar4/scan?

jarvisschultz gravatar image jarvisschultz  ( 2017-06-23 10:58:10 -0500 )edit

I used hector_model sonar sensor. By checking the frequency you mean "rostopic hz /r1/sonar4/scan" ? Because this command always return 10hz.

Hdifsttar gravatar image Hdifsttar  ( 2017-06-24 05:25:27 -0500 )edit

This may be a bug (or at least misleading documentation/examples) in hector_gazebo. Even though they set the update_rate parameters in the xacro files of hector_quadrotor_description. It doesn't look to me like the sonar plugin actually looks at the update_rate parameter.

jarvisschultz gravatar image jarvisschultz  ( 2017-06-26 09:01:12 -0500 )edit

In fact, you can see here that they hard-code the 10 Hz frequency of the timer that simulates and publishes the sonar topic.

jarvisschultz gravatar image jarvisschultz  ( 2017-06-26 09:02:31 -0500 )edit

So, you modifying the update_rate parameter should not do anything and we've found the reason that your /r1/sonar4/scan topic is running at 10Hz.

jarvisschultz gravatar image jarvisschultz  ( 2017-06-26 09:04:14 -0500 )edit

Thank you very much for having found this, now it all makes sense ! I'll try to change it in the .cpp file directly to see if it works.

edit : it worked fine after the change and a catkin_make of my workspace !

Hdifsttar gravatar image Hdifsttar  ( 2017-06-26 10:13:54 -0500 )edit

Your Answer

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

Add Answer

Question Tools

1 follower

Stats

Asked: 2017-06-23 09:32:11 -0500

Seen: 5,287 times

Last updated: Jun 23 '17