aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-02-11 09:20:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fixed Core Node still trying to use ports in anchor mode

Browse Source
This commit is contained in:
Tristan McGinnis
2025-02-20 11:41:42 -06:00
parent 095b97000d
commit 8e757464d1
4 changed files with 20 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
launch/__pycache__/rover_launch.cpython-312.pyc
View File

Binary file not shown.

4
src/anchor_pkg/anchor_pkg/anchor_node.py
View File

@@ -49,7 +49,7 @@ class SerialRelay(Node):
# if pong is in response, then we are talking with the MCU # if pong is in response, then we are talking with the MCU
if b"pong" in response: if b"pong" in response:
self.port = port self.port = port
self.get_logger.info(f"Found MCU at {self.port}!") self.get_logger().info(f"Found MCU at {self.port}!")
break break
except: except:
pass pass
@@ -57,7 +57,7 @@ class SerialRelay(Node):
break break
if self.port is None: if self.port is None:
self.get_logger.info("Unable to find MCU...") self.get_logger().info("Unable to find MCU...")
time.sleep(1) time.sleep(1)
sys.exit(1) sys.exit(1)

10
src/arm_pkg/arm_pkg/arm_node.py
View File

@@ -22,8 +22,8 @@ class SerialRelay(Node):
# Get launch mode parameter # Get launch mode parameter
self.declare_parameter('launch_mode', 'arm') self.declare_parameter('launch_mode', 'arm')
launch_mode = self.get_parameter('launch_mode').value self.launch_mode = self.get_parameter('launch_mode').value
self.get_logger().info(f"arm launch_mode is: {launch_mode}") self.get_logger().info(f"arm launch_mode is: {self.launch_mode}")
# Create publishers # Create publishers
self.debug_pub = self.create_publisher(String, '/arm/feedback/debug', 10) self.debug_pub = self.create_publisher(String, '/arm/feedback/debug', 10)
@@ -33,13 +33,13 @@ class SerialRelay(Node):
self.man_sub = self.create_subscription(ArmManual, '/arm/control/manual', self.send_manual, 10) self.man_sub = self.create_subscription(ArmManual, '/arm/control/manual', self.send_manual, 10)
# Topics used in anchor mode # Topics used in anchor mode
if launch_mode == 'anchor': if self.launch_mode == 'anchor':
self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10) self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10)
self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10) self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
# Search for ports IF in 'arm' (standalone) and not 'anchor' mode # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
if launch_mode == 'arm': if self.launch_mode == 'arm':
# Loop through all serial devices on the computer to check for the MCU # Loop through all serial devices on the computer to check for the MCU
self.port = None self.port = None
ports = SerialRelay.list_serial_ports() ports = SerialRelay.list_serial_ports()
@@ -150,7 +150,7 @@ class SerialRelay(Node):
#print(f"[Arm Wrote] {cmd}", end="") #print(f"[Arm Wrote] {cmd}", end="")
def anchor_feedback(self, msg): def anchor_feedback(self, msg):
self.get_logger.info(f"[Anchor] {msg.data}", end="") self.get_logger.info(f"[Arm Anchor] {msg.data}", end="")
#self.send_cmd(msg.data) #self.send_cmd(msg.data)

23
src/core_pkg/core_pkg/core_node.py
View File

@@ -25,8 +25,8 @@ class SerialRelay(Node):
# Get launch mode parameter # Get launch mode parameter
self.declare_parameter('launch_mode', 'core') self.declare_parameter('launch_mode', 'core')
launch_mode = self.get_parameter('launch_mode').value self.launch_mode = self.get_parameter('launch_mode').value
self.get_logger().info(f"core launch_mode is: {launch_mode}") self.get_logger().info(f"core launch_mode is: {self.launch_mode}")
# Create publishers # Create publishers
self.debug_pub = self.create_publisher(String, '/core/debug', 10) self.debug_pub = self.create_publisher(String, '/core/debug', 10)
@@ -37,12 +37,12 @@ class SerialRelay(Node):
# Create a service server for pinging the rover # Create a service server for pinging the rover
self.ping_service = self.create_service(Empty, '/astra/core/ping', self.ping_callback) self.ping_service = self.create_service(Empty, '/astra/core/ping', self.ping_callback)
if launch_mode == 'anchor': if self.launch_mode == 'anchor':
self.anchor_sub = self.create_subscription(String, '/anchor/core/feedback', self.anchor_feedback, 10) self.anchor_sub = self.create_subscription(String, '/anchor/core/feedback', self.anchor_feedback, 10)
self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10) self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
if launch_mode == 'core': if self.launch_mode == 'core':
# Loop through all serial devices on the computer to check for the MCU # Loop through all serial devices on the computer to check for the MCU
self.port = None self.port = None
ports = SerialRelay.list_serial_ports() ports = SerialRelay.list_serial_ports()
@@ -65,13 +65,13 @@ class SerialRelay(Node):
if self.port is not None: if self.port is not None:
break break
if self.port is None: if self.port is None:
self.get_logger.info("Unable to find MCU...") self.get_logger.info("Unable to find MCU...")
time.sleep(1) time.sleep(1)
sys.exit(1) sys.exit(1)
self.ser = serial.Serial(self.port, 115200) self.ser = serial.Serial(self.port, 115200)
atexit.register(self.cleanup) atexit.register(self.cleanup)
def run(self): def run(self):
@@ -180,6 +180,9 @@ class SerialRelay(Node):
elif self.launch_mode == 'core': elif self.launch_mode == 'core':
self.ser.write(bytes(cmd, "utf8")) self.ser.write(bytes(cmd, "utf8"))
def anchor_feedback(self, msg):
self.get_logger.info(f"[Arm Anchor] {msg.data}", end="")
def ping_callback(self, request, response): def ping_callback(self, request, response):
return response return response