fix for controller automatically reconnecting after disconnect, correct oneshot timer for controller feedback, moved rclpy.shutdown() in the SignalHandler

geoporus1 commented 3 years ago

Addresses #20

After further development on my personal project I came to the realization that there were still improvements to be done for the foxy-devel branch.

now the controller correctly re-connects if it is disconnected (in either USB or Bluetooth mode). If the controller is shut down or disconnected and then started again, the node correctly connects back to it. In order to handle this gracefully I am letting the device discovery to keep running in the main thread and I am doing the spinning in a new thread. The rclpy.shutdown() is then handled in the SIGINT handler in the ds4_driver_node.py. Previously the spinning was done right after the device was discovered and set-up and this rendered the re-discovery of the controller after disconnect unable to work properly since spinning was keeping the thread busy. I have tested many approaches which can be seen in the older commits on the working tree and thus far this method of spinning in a separate thread has been the best solution for this issue.
the previous one-shot timer fix I added in controller_ros.py had a mistake at the line where i was destroying it after the first callback call. This is now fixed as well and the one-shot timer for the feedback works as intended.

Sorry for the big number commits, these can be squished after the merge. I went through lots of experimentation in order to be able to have the device discovery and spin run correctly together and to also have the node die gracefully on SIGINT.

@naoki-mizuno I know this merge request might be a little confusing as I am not sure if the automatic controller re-discovery was a feature intended originally for this package. To me it has been really useful. Kindly check this whenever you can and let me know if any clarifications are needed. This is I believe the best and most stable version of the package for foxy and I would very much appreciate if it could be merged to the master repository so then I won't need to keep track of my personal forked version in the .repos of my bigger project.

Cheers!

naoki-mizuno commented 3 years ago

Thanks for contributing again! (Sorry for the late response; somehow the first email ended up being archived)

I'll try and test out your patch on a docker image within the next few days!

geoporus1 commented 3 years ago

@naoki-mizuno No worriez! Let me know how the tests go once you have time to do them!

geoporus1 commented 3 years ago

@naoki-mizuno I have just seen that my foxy-devel branch had some merge conflicts with your foxy-devel branch because I forgot to sync my fork after we merged the last changes I've made, before starting work on the new ones. I have now synced my fork, solved the conflicts and pushed to the branch. There should be no further conflicts now so if your tests end up being successful this can be more easily merged! :)

naoki-mizuno commented 3 years ago

I was able to test this PR on a docker image and confirmed that ds4_driver can properly reconnect to the device on USB. Although, I tried plugging out and plugging in the device with the latest foxy-devel branch (dff85862059b2c46803842dc469150692819067c) and found that ds4_driver can find the device with no problem. Could you elaborate more on when this problem occurs?

In the container I ran ros2 run ds4_driver ds4_driver_node.py --ros-args -p use_standard_msgs:=true -p autorepeat_rate:=10.

geoporus1 commented 3 years ago

Hello @naoki-mizuno and thanks for the quick reply! Now this is where things start getting weird. I have checked out the exact same commit you have specified there and with the same command specified by you there the controller does not reconnect for me. On top of this, when running the node with the default configuration, so with use_standard_msgs set to False and autorepeat_rate left to 0 the controller alos does not reconnect and sending feedback through the /set_feedback topic fails on second try due to the wrong way I am destroyng the one-shot timer in the previous fix. Not sure why the controller does not reconnect for me with the same command you ran..

geoporus1 commented 3 years ago

Can still confirm that with the latest patch in this pr the aforementioned issues do not appear anymore, in either modes so with or without use_standard_messages set to True and setting the feedback also works fine.

geoporus1 commented 3 years ago

To further clarify this issue, I believe that the place in code in ds4_driver_node.py where we are calling rclpy.spin(node) now in the commit specified above is not the correct place, because it blocks the main thread and the device re-discovery cannot possibly work if the spin is still running. Apart from this, the other changes are related to properly calling the correct rclpy.shutdown function and not rclpy.shutdown_now (which to may knowledge does not exist), and fixing the destroying of the one-shot timer for setting the feedback. :)

naoki-mizuno commented 3 years ago

Thanks for looking into this!

because it blocks the main thread and the device re-discovery cannot possibly work

I agree. There is no spin in the ROS1 version of the code because of that exact reason.

I'll go ahead and merge this PR into foxy-devel. Thanks for your contribution!

naoki-mizuno / ds4_driver

fix for controller automatically reconnecting after disconnect, correct oneshot timer for controller feedback, moved rclpy.shutdown() in the SignalHandler #19