mirror of
https://github.com/SHC-ASTRA/rover-ros2.git
synced 2026-04-20 03:41:17 -05:00
put the timeout back and figure out how shutdown works
This commit is contained in:
@@ -1,11 +1,9 @@
|
||||
from warnings import deprecated
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor
|
||||
from rclpy.executors import ExternalShutdownException, SingleThreadedExecutor
|
||||
from rcl_interfaces.msg import ParameterDescriptor, ParameterType
|
||||
|
||||
import atexit
|
||||
|
||||
from .connector import (
|
||||
Connector,
|
||||
MockConnector,
|
||||
@@ -186,11 +184,10 @@ class Anchor(Node):
|
||||
# poll at 100Hz for incoming data
|
||||
self.read_timer_ = self.create_timer(0.01, self.read_connector)
|
||||
|
||||
# close devices on exit
|
||||
atexit.register(self.cleanup)
|
||||
|
||||
def cleanup(self):
|
||||
def destroy_node(self):
|
||||
self.get_logger().info("closing connector")
|
||||
self.connector.cleanup()
|
||||
super().destroy_node()
|
||||
|
||||
def read_connector(self):
|
||||
"""Check the connector for new data from the MCU, and publish string to appropriate topics"""
|
||||
@@ -241,7 +238,7 @@ class Anchor(Node):
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
anchor_node = Anchor()
|
||||
executor = MultiThreadedExecutor()
|
||||
executor = SingleThreadedExecutor()
|
||||
executor.add_node(anchor_node)
|
||||
|
||||
try:
|
||||
@@ -249,7 +246,9 @@ def main(args=None):
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
# might not have to shut down everything manually, find out later ~riley
|
||||
# don't accept any more jobs
|
||||
executor.shutdown()
|
||||
# make the node quit processing things
|
||||
anchor_node.destroy_node()
|
||||
# shut down everything else
|
||||
rclpy.try_shutdown()
|
||||
|
||||
@@ -154,7 +154,7 @@ class SerialConnector(Connector):
|
||||
serial_interface: serial.Serial
|
||||
try:
|
||||
self.logger.info(f"asking {port} for its name")
|
||||
serial_interface = serial.Serial(port, BAUD_RATE)
|
||||
serial_interface = serial.Serial(port, BAUD_RATE, timeout=1)
|
||||
|
||||
serial_interface.write(b"can_relay_mode,on\n")
|
||||
|
||||
|
||||
Reference in New Issue
Block a user