ros-drivers / rosserial

A ROS client library for small, embedded devices, such as Arduino. See: http://wiki.ros.org/rosserial
517 stars 525 forks source link

SerialClient.py does not txStopRequest when exiting, as it is supposed to #549

Open altineller opened 3 years ago

altineller commented 3 years ago

Hello,

While I was migrating my device code from melodic to noetic, I found out that pressing Ctrl-C does not end the connection to the device. It will take 10-20 seconds before the device gets to configured=False state, and that is because of the time out.

I debugged the issue by putting logerror statements in node_handle.h, to see the txStop packet is actually making to the device, and that checksum is correct, etc. but I could not observe stop packet being transmitted when the SerialClient.py quits.

Then I put a statement in the loop:

while self.write_thread.is_alive() and not rospy.is_shutdown(): trigger += 1 if trigger % 1024 == 0: self.txStopRequest(); To send txStop requests each 1024 times the loop runs, and on the device side I was able to observe ID_TX_STOP = 11u. (0b in hex when sending)

So the problem is not protocol level, but in SerialClient.py. I think while transitioning from python2 to python3, something has changed and at the end of the run loop:

` except IOError as exc: rospy.logwarn('Last read step: %s' % read_step) rospy.logwarn('Run loop error: %s' % exc)

One of the read calls had an issue. Just to be safe, request that the client

            # reinitialize their topics.
            with self.read_lock:
                self.port.flushInput()
            with self.write_lock:
                self.port.flushOutput()
            self.requestTopics()
    self.txStopRequest()
    self.write_thread.join()

` I think the serial connection is dropped before the txStopRequest is called.

I debugged and isolated the problem, but I don't know how to fix it.

Any ideas/recommendations/help greatly appreciated.

altineller commented 3 years ago

Hello,

I came up with a fix, it works, but maybe not in the best form.

` self.publishers = dict() # id:Publishers self.subscribers = dict() # topic:Subscriber self.services = dict() # topic:Service

add this @ line 349

    def shutdown():
        self.txStopRequest()
        rospy.loginfo('shutdown hook activated')
    rospy.on_shutdown(shutdown)

    self.pub_diagnostics = rospy.Publisher('/diagnostics', diagnostic_msgs.msg.DiagnosticArray, queue_size=10

`

and I commented out the self.txStopRequest() at the end before the join thread.