Motor controller works only when I print using loginfo in cmd_vel callback

Question

I am using a Roboclaw Motor Controller which I control using software I have taken from Github and modified. I am publishing Twist messages using teleop_twist_joy. This works fine when I have the line rospy.loginfo("") in my cmd_vel callback but when I remove the loginfo I get latencies and generally weird behavior. For example the robot will stand still for a while before starting to move and it will also keep moving after I release the joypad activation button.

I have a callback on cmd_vel which looks like this:

        def cmd_vel_callback(self, twist):                                                                                                                                                                  
            if not self.paused:                                                                                                                                                                         
                    self.last_set_speed_time = rospy.get_rostime()                                                                                                                                      
                                                                                                                                                                                                        
                    linear_x = twist.linear.x                                                                                                                                                           
                    if linear_x >  self.MAX_SPEED: linear_x =  self.MAX_SPEED                                                                                                                           
                    if linear_x < -self.MAX_SPEED: linear_x = -self.MAX_SPEED                                                                                                                           
                                                                                                                                                                                                        
                    Vr = linear_x + twist.angular.z*self.BASE_WIDTH/2.0 # m/s                                                                                                                           
                    Vl = linear_x - twist.angular.z*self.BASE_WIDTH/2.0                                                                                                                                 
                                                                                                                                                                                                        
                    Vr_ticks = int(Vr * self.TICKS_PER_METER) # ticks/s                                                                                                                                 
                    Vl_ticks = int(Vl * self.TICKS_PER_METER)                                                                                                                                           
                                                                                                                                                                                                        
                    rospy.loginfo("")                                                                                                                                                                  
                                                                                                                                                                                                        
                    self.roboclaw.SetSpeed(Vl_ticks,Vr_ticks)

Notice the line rospy.loginfo(""), when I comment this things stop working. To make things work I can either uncomment that line again or have a second callback like this:

        def cmd_vel_log_callback(self, twist):                                                                                                                                                              
            rospy.loginfo("")

The code for subscribing that I use is:

            rospy.Subscriber('cmd_vel', Twist, self.cmd_vel_callback)                                                                                                                                   
            rospy.Subscriber('cmd_vel', Twist, self.cmd_vel_log_callback)

This is of course unsatisfactory and I would like to understand how I can get things working without the extra rospy.loginfo("").

I am using melodic on Ubuntu 18.04.2 LTS.

---

Originally posted by robotjohan on ROS Answers with karma: 15 on 2019-04-25

Post score: 0

Answer 1

It is very likely that self.roboclaw.SetSpeed(Vl_ticks,Vr_ticks) just cannot be called as fast as it would be if you comment the rospy.loginfo("") line. By having that line there, as a side-effect, you delay the call to SetSpeed(..) by some milliseconds giving something time to actually do some work.

re: motion after letting go of the button: that sounds like your node processing a backlog of incoming messages. You'll want to configure your subscriber such that it doesn't maintain any queue, so only the most recent message is processed.

---

Originally posted by gvdhoorn with karma: 86574 on 2019-04-25

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by robotjohan on 2019-04-26:
That seems very reasonable.

Do you have any suggestion on a good way of limiting the rate? I guess I could just scrap messages if they arrive to soon. This makes me worry that I will miss important messages, say a stop message, if it arrives untimely. I could then keep a log of the most recent message and make sure it is sent when possible. It seems my issue then is that I would like to know if there is a ROS way of doing this, or if I should just make it work somehow (maybe that is the ROS way?).

I'll also look into whether I keep a queue!

Thank you for your help!

Comment by gvdhoorn on 2019-04-26:\

Do you have any suggestion on a good way of limiting the rate?

While I'm always a fan of making ROS nodes completely event-based (ie: do work when msgs come in), in this case a periodic while with a ros::Rate and a copy of "the latest" message (as a member variable fi) might be an OK approach.

To make all of this less guesswork, you might want to see whether you can find some information on SetSpeed(..) and the rest of the roboclaw library you're using. Perhaps there are some specs on the runtime behaviour that explain all of this.

Right now we're just guessing, which makes suggesting an architecture/design rather difficult.

Comment by robotjohan on 2019-04-26:
I didn't manage to get any information on how often I can call SetSpeed. For now I went ahead and implemented a variable which stores the values from the last message and send an update to the motor controller with a frequency of 100 Hz which seems to work fine.

I also changed the queue size to 1 which did the trick.

Again, thank you for your help!