aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-04-20 20:01:15 -05:00
Code Issues Packages Projects Releases Wiki Activity

refactor(core): cleanup to match arm

Browse Source
This commit is contained in:
David
2026-03-20 10:42:18 -05:00
parent ff4a58e6ed
commit a17060ceda
2 changed files with 119 additions and 211 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,5 +1,4 @@
import sys import sys
import threading
import signal import signal
import math import math
from warnings import deprecated from warnings import deprecated
@@ -26,8 +25,6 @@ control_qos = qos.QoSProfile(
# liveliness_lease_duration=Duration(seconds=5), # liveliness_lease_duration=Duration(seconds=5),
) )
thread = None
class ArmNode(Node): class ArmNode(Node):
"""Relay between Anchor and Basestation/Headless/Moveit2 for Arm related topics.""" """Relay between Anchor and Basestation/Headless/Moveit2 for Arm related topics."""

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

@@ -1,20 +1,14 @@
import rclpy
from rclpy.node import Node
from rclpy import qos
from rclpy.duration import Duration
from std_srvs.srv import Empty
import signal
import time
import atexit
import serial
import os
import sys import sys
import threading import signal
import glob
from scipy.spatial.transform import Rotation from scipy.spatial.transform import Rotation
from math import copysign, pi from math import copysign, pi
from warnings import deprecated
import rclpy
from rclpy.node import Node
from rclpy.executors import ExternalShutdownException
from rclpy import qos
from rclpy.duration import Duration
from std_msgs.msg import String, Header from std_msgs.msg import String, Header
from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus, JointState from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus, JointState
@@ -23,9 +17,6 @@ from astra_msgs.msg import CoreControl, CoreFeedback, RevMotorState
from astra_msgs.msg import VicCAN, NewCoreFeedback, Barometer, CoreCtrlState from astra_msgs.msg import VicCAN, NewCoreFeedback, Barometer, CoreCtrlState
serial_pub = None
thread = None
CORE_WHEELBASE = 0.836 # meters CORE_WHEELBASE = 0.836 # meters
CORE_WHEEL_RADIUS = 0.171 # meters CORE_WHEEL_RADIUS = 0.171 # meters
CORE_GEAR_RATIO = 100.0 # Clucky: 100:1, Testbed: 64:1 CORE_GEAR_RATIO = 100.0 # Clucky: 100:1, Testbed: 64:1
@@ -41,6 +32,10 @@ control_qos = qos.QoSProfile(
# liveliness_lease_duration=Duration(seconds=5), # liveliness_lease_duration=Duration(seconds=5),
) )
class CoreNode(Node):
"""Relay between Anchor and Basestation/Headless/Moveit2 for Core related topics."""
# Used to verify the length of an incoming VicCAN feedback message # Used to verify the length of an incoming VicCAN feedback message
# Key is VicCAN command_id, value is expected length of data list # Key is VicCAN command_id, value is expected length of data list
viccan_msg_len_dict = { viccan_msg_len_dict = {
@@ -55,26 +50,45 @@ viccan_msg_len_dict = {
58: 4, # ditto 58: 4, # ditto
} }
class SerialRelay(Node):
def __init__(self): def __init__(self):
# Initalize node with name
super().__init__("core_node") super().__init__("core_node")
# Launch mode -- anchor vs core self.get_logger().info(f"core launch_mode is: anchor")
self.declare_parameter("launch_mode", "core")
self.launch_mode = self.get_parameter("launch_mode").value
self.get_logger().info(f"Core launch_mode is: {self.launch_mode}")
self.declare_parameter("use_ros2_control", False)
self.use_ros2_control = self.get_parameter("use_ros2_control").value
self.get_logger().info(f"Use ros2_control: {self.use_ros2_control}")
################################################## ##################################################
# Topics # Parameters
self.declare_parameter("use_ros2_control", False)
self.use_ros2_control = (
self.get_parameter("use_ros2_control").get_parameter_value().bool_value
)
##################################################
# Old Topics
self.anchor_sub = self.create_subscription(
String, "/anchor/core/feedback", self.anchor_feedback, 10
)
self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
if not self.use_ros2_control:
# /core/control
self.control_sub = self.create_subscription(
CoreControl, "/core/control", self.send_controls, 10
) # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
# /core/feedback
self.feedback_pub = self.create_publisher(CoreFeedback, "/core/feedback", 10)
self.core_feedback = CoreFeedback()
self.telemetry_pub_timer = self.create_timer(
1.0, self.publish_feedback
)
##################################################
# New Topics
# Anchor # Anchor
if self.launch_mode == "anchor":
self.anchor_fromvic_sub_ = self.create_subscription( self.anchor_fromvic_sub_ = self.create_subscription(
VicCAN, "/anchor/from_vic/core", self.relay_fromvic, 20 VicCAN, "/anchor/from_vic/core", self.relay_fromvic, 20
) )
@@ -82,11 +96,6 @@ class SerialRelay(Node):
VicCAN, "/anchor/to_vic/relay", 20 VicCAN, "/anchor/to_vic/relay", 20
) )
self.anchor_sub = self.create_subscription(
String, "/anchor/core/feedback", self.anchor_feedback, 10
)
self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
# Control # Control
if self.use_ros2_control: if self.use_ros2_control:
@@ -100,6 +109,7 @@ class SerialRelay(Node):
TwistStamped, "/cmd_vel", self.cmd_vel_callback, 1 TwistStamped, "/cmd_vel", self.cmd_vel_callback, 1
) )
# manual twist -- [-1, 1] rather than real units # manual twist -- [-1, 1] rather than real units
# TODO: change topic to '/core/control/twist'
self.twist_man_sub_ = self.create_subscription( self.twist_man_sub_ = self.create_subscription(
Twist, "/core/twist", self.twist_man_callback, qos_profile=control_qos Twist, "/core/twist", self.twist_man_callback, qos_profile=control_qos
) )
@@ -114,143 +124,60 @@ class SerialRelay(Node):
# Feedback # Feedback
# Consolidated and organized core feedback # Consolidated and organized main core feedback
# TODO: change topic to something like '/core/feedback/main'
self.feedback_new_pub_ = self.create_publisher( self.feedback_new_pub_ = self.create_publisher(
NewCoreFeedback, NewCoreFeedback,
"/core/feedback_new", "/core/feedback_new",
qos_profile=qos.qos_profile_sensor_data, qos_profile=qos.qos_profile_sensor_data,
) )
# Joint states for topic-based controller
self.joint_state_pub_ = self.create_publisher(
JointState, "/joint_states", qos_profile=qos.qos_profile_sensor_data
)
# IMU (embedded BNO-055)
self.imu_pub_ = self.create_publisher(
Imu, "/core/imu", qos_profile=qos.qos_profile_sensor_data
)
# GPS (embedded u-blox M9N)
self.gps_pub_ = self.create_publisher(
NavSatFix, "/gps/fix", qos_profile=qos.qos_profile_sensor_data
)
# Barometer (embedded BMP-388)
self.baro_pub_ = self.create_publisher(
Barometer, "/core/feedback/baro", qos_profile=qos.qos_profile_sensor_data
)
###################################################
# Saved state
# Main Core feedback
self.feedback_new_state = NewCoreFeedback() self.feedback_new_state = NewCoreFeedback()
self.feedback_new_state.fl_motor.id = 1 self.feedback_new_state.fl_motor.id = 1
self.feedback_new_state.bl_motor.id = 2 self.feedback_new_state.bl_motor.id = 2
self.feedback_new_state.fr_motor.id = 3 self.feedback_new_state.fr_motor.id = 3
self.feedback_new_state.br_motor.id = 4 self.feedback_new_state.br_motor.id = 4
self.telemetry_pub_timer = self.create_timer(
1.0, self.publish_feedback # IMU
) # TODO: not sure about this
# Joint states for topic-based controller
self.joint_state_pub_ = self.create_publisher(
JointState, "/joint_states", qos_profile=qos.qos_profile_sensor_data
)
# IMU (embedded BNO-055)
self.imu_pub_ = self.create_publisher(
Imu, "/core/imu", qos_profile=qos.qos_profile_sensor_data
)
self.imu_state = Imu() self.imu_state = Imu()
self.imu_state.header.frame_id = "core_bno055" self.imu_state.header.frame_id = "core_bno055"
# GPS (embedded u-blox M9N)
self.gps_pub_ = self.create_publisher( # GPS
NavSatFix, "/gps/fix", qos_profile=qos.qos_profile_sensor_data
)
self.gps_state = NavSatFix() self.gps_state = NavSatFix()
self.gps_state.header.frame_id = "core_gps_antenna" self.gps_state.header.frame_id = "core_gps_antenna"
self.gps_state.status.service = NavSatStatus.SERVICE_GPS self.gps_state.status.service = NavSatStatus.SERVICE_GPS
self.gps_state.status.status = NavSatStatus.STATUS_NO_FIX self.gps_state.status.status = NavSatStatus.STATUS_NO_FIX
self.gps_state.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN self.gps_state.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
# Barometer (embedded BMP-388)
self.baro_pub_ = self.create_publisher( # Barometer
Barometer, "/core/baro", qos_profile=qos.qos_profile_sensor_data
)
self.baro_state = Barometer() self.baro_state = Barometer()
self.baro_state.header.frame_id = "core_bmp388" self.baro_state.header.frame_id = "core_bmp388"
# Old @deprecated("Uses an old message type. Will be removed at some point.")
if not self.use_ros2_control:
# /core/control
self.control_sub = self.create_subscription(
CoreControl, "/core/control", self.send_controls, 10
) # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
# /core/feedback
self.feedback_pub = self.create_publisher(CoreFeedback, "/core/feedback", 10)
self.core_feedback = CoreFeedback()
# Debug
self.debug_pub = self.create_publisher(String, "/core/debug", 10)
self.ping_service = self.create_service(
Empty, "/astra/core/ping", self.ping_callback
)
##################################################
# Find microcontroller (Non-anchor only)
# Core (non-anchor) specific
if self.launch_mode == "core":
# Loop through all serial devices on the computer to check for the MCU
self.port = None
ports = SerialRelay.list_serial_ports()
for i in range(2):
for port in ports:
try:
# connect and send a ping command
ser = serial.Serial(port, 115200, timeout=1)
# (f"Checking port {port}...")
ser.write(b"ping\n")
response = ser.read_until("\n") # type: ignore
# if pong is in response, then we are talking with the MCU
if b"pong" in response:
self.port = port
self.get_logger().info(f"Found MCU at {self.port}!")
self.get_logger().info(f"Enabling Relay Mode")
ser.write(b"can_relay_mode,on\n")
break
except:
pass
if self.port is not None:
break
if self.port is None:
self.get_logger().info("Unable to find MCU...")
time.sleep(1)
sys.exit(1)
self.ser = serial.Serial(self.port, 115200)
atexit.register(self.cleanup)
# end __init__()
def run(self):
# This thread makes all the update processes run in the background
global thread
thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
thread.start()
try:
while rclpy.ok():
if self.launch_mode == "core":
self.read_MCU() # Check the MCU for updates
except KeyboardInterrupt:
sys.exit(0)
def read_MCU(self): # NON-ANCHOR SPECIFIC
try:
output = str(self.ser.readline(), "utf8")
if output:
# All output over debug temporarily
print(f"[MCU] {output}")
msg = String()
msg.data = output
self.debug_pub.publish(msg)
return
except serial.SerialException as e:
print(f"SerialException: {e}")
print("Closing serial port.")
if self.ser.is_open:
self.ser.close()
sys.exit(1)
except TypeError as e:
print(f"TypeError: {e}")
print("Closing serial port.")
if self.ser.is_open:
self.ser.close()
sys.exit(1)
except Exception as e:
print(f"Exception: {e}")
print("Closing serial port.")
if self.ser.is_open:
self.ser.close()
sys.exit(1)
def scale_duty(self, value: float, max_speed: float): def scale_duty(self, value: float, max_speed: float):
leftMin = -1 leftMin = -1
leftMax = 1 leftMax = 1
@@ -267,6 +194,7 @@ class SerialRelay(Node):
# Convert the 0-1 range into a value in the right range. # Convert the 0-1 range into a value in the right range.
return str(rightMin + (valueScaled * rightSpan)) return str(rightMin + (valueScaled * rightSpan))
@deprecated("Uses an old message type. Will be removed at some point.")
def send_controls(self, msg: CoreControl): def send_controls(self, msg: CoreControl):
if msg.turn_to_enable: if msg.turn_to_enable:
command = ( command = (
@@ -377,15 +305,9 @@ class SerialRelay(Node):
# Max duty cycle # Max duty cycle
self.twist_max_duty = msg.max_duty # twist_man_callback will handle this self.twist_max_duty = msg.max_duty # twist_man_callback will handle this
@deprecated("Uses an old message type. Will be removed at some point.")
def send_cmd(self, msg: str): def send_cmd(self, msg: str):
if self.launch_mode == "anchor": self.anchor_pub.publish(String(data=msg)) # Publish to anchor for relay
# self.get_logger().info(f"[Core to Anchor Relay] {msg}")
output = String() # Convert to std_msg string
output.data = msg
self.anchor_pub.publish(output)
elif self.launch_mode == "core":
self.get_logger().info(f"[Core to MCU] {msg}")
self.ser.write(bytes(msg, "utf8"))
def send_viccan(self, cmd_id: int, data: list[float]): def send_viccan(self, cmd_id: int, data: list[float]):
self.anchor_tovic_pub_.publish( self.anchor_tovic_pub_.publish(
@@ -397,6 +319,7 @@ class SerialRelay(Node):
) )
) )
@deprecated("Uses an old message type. Will be removed at some point.")
def anchor_feedback(self, msg: String): def anchor_feedback(self, msg: String):
output = msg.data output = msg.data
parts = str(output.strip()).split(",") parts = str(output.strip()).split(",")
@@ -466,8 +389,8 @@ class SerialRelay(Node):
# skill diff if not # skill diff if not
# Check message len to prevent crashing on bad data # Check message len to prevent crashing on bad data
if msg.command_id in viccan_msg_len_dict: if msg.command_id in self.viccan_msg_len_dict:
expected_len = viccan_msg_len_dict[msg.command_id] expected_len = self.viccan_msg_len_dict[msg.command_id]
if len(msg.data) != expected_len: if len(msg.data) != expected_len:
self.get_logger().warning( self.get_logger().warning(
f"Ignoring VicCAN message with id {msg.command_id} due to unexpected data length (expected {expected_len}, got {len(msg.data)})" f"Ignoring VicCAN message with id {msg.command_id} due to unexpected data length (expected {expected_len}, got {len(msg.data)})"
@@ -597,31 +520,11 @@ class SerialRelay(Node):
case _: case _:
return return
@deprecated("Uses an old message type. Will be removed at some point.")
def publish_feedback(self): def publish_feedback(self):
# self.get_logger().info(f"[Core] {self.core_feedback}") # self.get_logger().info(f"[Core] {self.core_feedback}")
self.feedback_pub.publish(self.core_feedback) self.feedback_pub.publish(self.core_feedback)
def ping_callback(self, request, response):
return response
@staticmethod
def list_serial_ports():
return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
def cleanup(self):
print("Cleaning up before terminating...")
try:
if self.ser.is_open:
self.ser.close()
except Exception as e:
exit(0)
def myexcepthook(type, value, tb):
print("Uncaught exception:", type, value)
if serial_pub:
serial_pub.cleanup()
def map_range( def map_range(
value: float, in_min: float, in_max: float, out_min: float, out_max: float value: float, in_min: float, in_max: float, out_min: float, out_max: float
@@ -633,19 +536,27 @@ def radps_to_rpm(radps: float):
return radps * 60 / (2 * pi) return radps * 60 / (2 * pi)
def exit_handler(signum, frame):
print("Caught SIGTERM. Exiting...")
rclpy.try_shutdown()
sys.exit(0)
def main(args=None): def main(args=None):
rclpy.init(args=args) rclpy.init(args=args)
sys.excepthook = myexcepthook
global serial_pub # Catch termination signals and exit cleanly
signal.signal(signal.SIGTERM, exit_handler)
serial_pub = SerialRelay() core_node = CoreNode()
serial_pub.run()
try:
rclpy.spin(core_node)
except (KeyboardInterrupt, ExternalShutdownException):
pass
finally:
rclpy.try_shutdown()
if __name__ == "__main__": if __name__ == "__main__":
# signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0)) # Catch Ctrl+Z and exit cleanly
signal.signal(
signal.SIGTERM, lambda signum, frame: sys.exit(0)
) # Catch termination signals and exit cleanly
main() main()