Wait for encoder ticks being published in ROS control hardware interface

[fjp]

Use ros::Subscriber::getNumPublishers( ) inside the hardware interface to check if encoder ticks are received.

Before starting to control loop (read(), update(), write()) inside the hardware interface, we can use its subscriber to wait for the encoder tick messages being published.

Inside the constructor of the hardware interface we call the new method:

https://github.com/fjp/diffbot/blob/3ccba421f1d256c491dff9f441172173dcb05c59/diffbot_base/src/diffbot_hw_interface.cpp#L51-L52

This method is defined like this:

https://github.com/fjp/diffbot/blob/3ccba421f1d256c491dff9f441172173dcb05c59/diffbot_base/src/diffbot_hw_interface.cpp#L215-L237

Closes #30

[fjp]

Adding isReceivingEncoderTicks method helps to solve this problem when the robot (including teensy mcu) is powerd up for the first time. No uncontrolled wheel rotations happen after the launch (roslaunch diffbot_base bringup.launch) and when driving the robot around, the encoder ticks increase as expected.

The problem is still present when the diffbot_base hardware interface is stopped and restarted (Ctrl + C followed by roslaunch diffbot_base bringup.launch). In this case the encoder ticks didn't reset and the last value from the previous execution is stored and published from the teensy mcu. This results again in a jump: encoder tick values in the hardware intf are initialized to zero, while the values read from the teensy mcu are the ones from the last run. This results in a high delta angle and therefore high velocity, which in turn leads to a (unwanted) high output to the motors.
Here is the output of this problem:

[ INFO] [1618225424.652409780]: ... Done Initializing DiffBot Hardware Interface
[ INFO] [1618225424.652618887]: Get number of encoder ticks publishers
[ INFO] [1618225424.652764272]: Waiting for encoder ticks being published...
[INFO] [1618225425.516883]: Ready to receive motor commands
[INFO] [1618225425.810446]: ROS Serial Python Node
[INFO] [1618225425.843429]: Connecting to /dev/ttyACM0 at 115200 baud
[INFO] [1618225425.867516]: Controller Spawner: Waiting for service controller_manager/load_controller
[INFO] [1618225427.964794]: Requesting topics...
[INFO] [1618225427.988818]: Note: publish buffer size is 512 bytes
[INFO] [1618225427.996640]: Setup publisher on encoder_ticks [diffbot_msgs/Encoder]
[INFO] [1618225428.014850]: Note: subscribe buffer size is 512 bytes
[INFO] [1618225428.022365]: Setup subscriber on reset [std_msgs/Empty]
[ INFO] [1618225429.567816663]: Number of encoder ticks publishers: 1
[ INFO] [1618225429.797155154]: 
Read      ticks     angle     dangle    velocity  
j0:       21466     248.847   248.847   2486.09   
j1:       21544     249.751   249.751   2495.13   
[ INFO] [1618225429.797760343]: 
Write     velocity  p_error   i_error   d_error   pid out   percent   
j0:       0         -2486.09  -248.847  -24837.3  -1122.01  -18232.6  
j1:       0         -2495.13  -249.751  -24927.5  -1126.07  -18298.6

1. One way to fix this, is to send the /reset message from the hardware interface to the encoders.
2. Another way could be to initialize the encoder tick values inside the hardware interface with the values received from the encoders. However, this way the encoders won't be reset after re-launching the bringup.launch.

So I will check if it is possible to fix this using 1. by sending the /reset message.