From 3bdef8b59187492237a6c9a4eec79031f23d885b Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Tue, 18 Feb 2025 17:58:09 -0600 Subject: [PATCH 01/33] Flip manual control directions --- src/arm_pkg/arm_pkg/arm_headless.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_headless.py b/src/arm_pkg/arm_pkg/arm_headless.py index 761cbe3..c5f5721 100755 --- a/src/arm_pkg/arm_pkg/arm_headless.py +++ b/src/arm_pkg/arm_pkg/arm_headless.py @@ -113,9 +113,9 @@ class Headless(Node): elif dpad_input[0] == -1: input.axis0 = -1 - input.axis1 = round(self.gamepad.get_axis(0))#left x-axis - input.axis2 = round(self.gamepad.get_axis(1))#left y-axis - input.axis3 = round(self.gamepad.get_axis(4))#right y-axis + input.axis1 = -1 * round(self.gamepad.get_axis(0))#left x-axis + input.axis2 = -1 * round(self.gamepad.get_axis(1))#left y-axis + input.axis3 = -1 * round(self.gamepad.get_axis(4))#right y-axis #Temporary, not controlling digit. Awaiting embedded implementation input.effector_yaw = 0 From a0e4649c139c3c0dae99eb6408e643374f02dd60 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Wed, 19 Feb 2025 20:09:25 -0600 Subject: [PATCH 02/33] created anchor package and launch file --- .../__pycache__/rover_launch.cpython-312.pyc | Bin 0 -> 1911 bytes launch/rover_launch.py | 100 ++++++++++++++++++ src/anchor_pkg/anchor_pkg/__init__.py | 0 src/anchor_pkg/package.xml | 20 ++++ src/anchor_pkg/resource/anchor_pkg | 0 src/anchor_pkg/setup.cfg | 4 + src/anchor_pkg/setup.py | 24 +++++ 7 files changed, 148 insertions(+) create mode 100644 launch/__pycache__/rover_launch.cpython-312.pyc create mode 100644 launch/rover_launch.py create mode 100644 src/anchor_pkg/anchor_pkg/__init__.py create mode 100644 src/anchor_pkg/package.xml create mode 100644 src/anchor_pkg/resource/anchor_pkg create mode 100644 src/anchor_pkg/setup.cfg create mode 100644 src/anchor_pkg/setup.py diff --git a/launch/__pycache__/rover_launch.cpython-312.pyc b/launch/__pycache__/rover_launch.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..a7099b21b0a6e223d8191f3ba9d5e378739c4565 GIT binary patch literal 1911 zcmbtV&2Jk;6rc5e+Pik+B<=@p;!T?mEG2FcsgO{MfCeN~V-P_cEQGc3P8>J(?lQZZ zrd1;Oz=8Hadn;$eAt*WXPjGQ*C03(UL~1YGEF3O9@n&~zB|=duF{_<@GxO%Rzc>4P zZ+}atQwZ8mKm6LLY6!hxgF#EgMt=#6$H+t`*FtTs%5lt2v=VK;%D0885RUnl*p{kN zXbUa5tyC4T#qUrxX-d$i?#e~wF9uoUXtr$lR()*=yLHlZyryjx6EwGk>n(%eXzc^i z@Y~q(Xy!A=_}0fC!-mk2qhpcBP1{;+Hhf~l(?Z2Iah;1aq4BUMRvyYKCTr;`d z*uKp($K!1a4OX}L2HF<3cr&s7CPHLwOSml__hlnb9?Y4%DZnL~(iY$38r+_|8~YCs zIdm$9VkY7Ge;6q<0oN%Q%LpPl@QFpH+5SFxEQPat;?$w5c?m8N~~#wXT> z(Q2AHW2HO9-e{WGEa^AbY}>`UZNa_zt`6kpb%r*t!>)Nff-Dt9A~7i0!DQ8D0T_;h zEt4>vQPCky%VPo5ZOg-(9u*)UmrCn**pskhqM`*ac3mbuowZlzOIM=kuK*cLYSEzN z+@)H4DlYbXhv=|Ff!qBM5~PD(s*^u%JXL4^P)kqL($1Zy>N}mKK+AMKI*`@xH-DGU z9VqFC+I?+b8wt)&1$jNly%C%%1QW%b#qPzc-TYgJs+7(g@=7w*8%4>?(D_o3p9*sM z6P>4zI!_1r%Rz49MCY$Lq-8th-YpKLGTrRlljPiidgjr!2R8yO`>=e!ygxD?T$~9C z)4_#FhBdXY<8}4LZs8j6%Z>uSbf&iyqr8nWqmRCN@XgV^vq51dxKKE8@70jNp$M$I znNOc7l>>F8Q|`l2qB`pajeS{GbNXLGcf2M+#`1f@Zd{SJkvFVOTgV&M9giV|ZBHEJ zK_fA+ARDAgr~$&N-@Vy#sQFNc{}r4~V$DJMO`CYt@LOIDl7buzF z>|`*0{ZJOhq|U8F4QUgBGPbXr{aJgaEc8TvQtBgKlY594WikaDD@Bz`k&HuLG!x6) z2!9!F#F&w#4@@f^qfBOz|)PxStCwD=rd{#*E*Vo0{{q{p&t(7r literal 0 HcmV?d00001 diff --git a/launch/rover_launch.py b/launch/rover_launch.py new file mode 100644 index 0000000..2e2a759 --- /dev/null +++ b/launch/rover_launch.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python3 + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def launch_setup(context, *args, **kwargs): + # Retrieve the resolved value of the launch argument 'mode' + mode = LaunchConfiguration('mode').perform(context) + nodes = [] + + if mode == 'anchor': + # Launch every node and pass "anchor" as the parameter + nodes.append( + Node( + package='arm_pkg', + executable='arm', # change as needed + name='arm', + output='screen', + parameters=[{'launch_arg': mode}] + ) + ) + nodes.append( + Node( + package='core_pkg', + executable='core', # change as needed + name='core', + output='screen', + parameters=[{'launch_arg': mode}] + ) + ) + # nodes.append( + # Node( + # package='bio_pkg', + # executable='bio', # change as needed + # name='bio', + # output='screen', + # parameters=[{'launch_arg': mode}] + # ) + # ) + nodes.append( + Node( + package='anchor_pkg', + executable='anchor', # change as needed + name='anchor', + output='screen', + parameters=[{'launch_arg': mode}] + ) + ) + elif mode in ['arm', 'core', 'bio']: + # Only launch the node corresponding to the provided mode. + if mode == 'arm': + nodes.append( + Node( + package='arm_pkg', + executable='arm', + name='arm', + output='screen', + parameters=[{'launch_arg': mode}] + ) + ) + elif mode == 'core': + nodes.append( + Node( + package='core_pkg', + executable='core', + name='core', + output='screen', + parameters=[{'launch_arg': mode}] + ) + ) + # elif mode == 'bio': + # nodes.append( + # Node( + # package='bio_pkg', + # executable='bio', + # name='bio', + # output='screen', + # parameters=[{'launch_arg': mode}] + # ) + # ) + else: + # If an invalid mode is provided, print an error. + # (You might want to raise an exception or handle it differently.) + print("Invalid mode provided. Choose one of: arm, core, bio, anchor.") + + return nodes + +def generate_launch_description(): + declare_arg = DeclareLaunchArgument( + 'mode', + default_value='anchor', + description='Launch mode: arm, core, bio, or anchor' + ) + + return LaunchDescription([ + declare_arg, + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/anchor_pkg/anchor_pkg/__init__.py b/src/anchor_pkg/anchor_pkg/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/anchor_pkg/package.xml b/src/anchor_pkg/package.xml new file mode 100644 index 0000000..8366b03 --- /dev/null +++ b/src/anchor_pkg/package.xml @@ -0,0 +1,20 @@ + + + + anchor_pkg + 0.0.0 + TODO: Package description + tristan + TODO: License declaration + + rclpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/anchor_pkg/resource/anchor_pkg b/src/anchor_pkg/resource/anchor_pkg new file mode 100644 index 0000000..e69de29 diff --git a/src/anchor_pkg/setup.cfg b/src/anchor_pkg/setup.cfg new file mode 100644 index 0000000..9a77caa --- /dev/null +++ b/src/anchor_pkg/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/anchor_pkg +[install] +install_scripts=$base/lib/anchor_pkg diff --git a/src/anchor_pkg/setup.py b/src/anchor_pkg/setup.py new file mode 100644 index 0000000..376e0a1 --- /dev/null +++ b/src/anchor_pkg/setup.py @@ -0,0 +1,24 @@ +from setuptools import find_packages, setup + +package_name = 'anchor_pkg' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='tristan', + maintainer_email='tristanmcginnis26@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + entry_points={ + 'console_scripts': [ + ], + }, +) From 7a0d73394d103cb798bd15540f0dff58fd610b27 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Wed, 19 Feb 2025 22:49:15 -0600 Subject: [PATCH 03/33] add launch mode logic --- launch/rover_launch.py | 14 +- src/arm_pkg/arm_pkg/arm_node.py | 228 +++++++++-------------------- src/core_pkg/core_pkg/core_node.py | 82 +++++++---- 3 files changed, 125 insertions(+), 199 deletions(-) diff --git a/launch/rover_launch.py b/launch/rover_launch.py index 2e2a759..95bf004 100644 --- a/launch/rover_launch.py +++ b/launch/rover_launch.py @@ -18,7 +18,7 @@ def launch_setup(context, *args, **kwargs): executable='arm', # change as needed name='arm', output='screen', - parameters=[{'launch_arg': mode}] + parameters=[{'launch_mode': mode}] ) ) nodes.append( @@ -27,7 +27,7 @@ def launch_setup(context, *args, **kwargs): executable='core', # change as needed name='core', output='screen', - parameters=[{'launch_arg': mode}] + parameters=[{'launch_mode': mode}] ) ) # nodes.append( @@ -36,7 +36,7 @@ def launch_setup(context, *args, **kwargs): # executable='bio', # change as needed # name='bio', # output='screen', - # parameters=[{'launch_arg': mode}] + # parameters=[{'launch_mode': mode}] # ) # ) nodes.append( @@ -45,7 +45,7 @@ def launch_setup(context, *args, **kwargs): executable='anchor', # change as needed name='anchor', output='screen', - parameters=[{'launch_arg': mode}] + parameters=[{'launch_mode': mode}] ) ) elif mode in ['arm', 'core', 'bio']: @@ -57,7 +57,7 @@ def launch_setup(context, *args, **kwargs): executable='arm', name='arm', output='screen', - parameters=[{'launch_arg': mode}] + parameters=[{'launch_mode': mode}] ) ) elif mode == 'core': @@ -67,7 +67,7 @@ def launch_setup(context, *args, **kwargs): executable='core', name='core', output='screen', - parameters=[{'launch_arg': mode}] + parameters=[{'launch_mode': mode}] ) ) # elif mode == 'bio': @@ -77,7 +77,7 @@ def launch_setup(context, *args, **kwargs): # executable='bio', # name='bio', # output='screen', - # parameters=[{'launch_arg': mode}] + # parameters=[{'launch_mode': mode}] # ) # ) else: diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 45d60e2..8d70e06 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -17,76 +17,76 @@ thread = None class SerialRelay(Node): def __init__(self): - # Initialize node with name + # Initialize node super().__init__("arm_node") + # Get launch mode parameter + self.declare_parameter('launch_mode', 'arm') + launch_mode = self.get_parameter('launch_mode').value + self.get_logger().info(f"arm launch_mode is: {launch_mode}") + # Create publishers - - #Depricated, temporary for reference - # self.output_publisher = self.create_publisher(String, '/astra/arm/feedback', 10) - # self.state_publisher = self.create_publisher(ArmState, '/astra/arm/state', 10) - # self.faerie_publisher = self.create_publisher(FaerieTelemetry, '/astra/arm/bio/feedback', 10) - self.debug_pub = self.create_publisher(String, '/arm/feedback/debug', 10) self.socket_pub = self.create_publisher(SocketFeedback, '/arm/feedback/socket', 10) - - # Create subscribers - - #Depricated, temporary for reference - #self.control_subscriber = self.create_subscription(ControllerState, '/astra/arm/control', self.send_controls, 10) - self.ik_sub = self.create_subscription(ArmIK, '/arm/control/ik', self.send_ik, 10) self.man_sub = self.create_subscription(ArmManual, '/arm/control/manual', self.send_manual, 10) - #self.command_subscriber = self.create_subscription(String, '/astra/arm/command', self.send_command, 10) - #self.faerie_subscriber = self.create_subscription(String, "/astra/arm/bio/control", self.send_faerie_controls, 10) + # Topics used in anchor mode + if launch_mode == 'anchor': + self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10) + self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10) - # 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) - print(f"Checking port {port}...") - ser.write(b"ping\n") - response = ser.read_until("\n") - # if pong is in response, then we are talking with the MCU - if b"pong" in response: - self.port = port - print(f"Found MCU at {self.port}!") - break - except: - pass - if self.port is not None: - break - - if self.port is None: - print("Unable to find MCU... please make sure it is connected.") - time.sleep(1) - sys.exit(1) - - self.ser = serial.Serial(self.port, 115200) - atexit.register(self.cleanup) + # Search for ports IF in 'arm' (standalone) and not 'anchor' mode + if launch_mode == 'arm': + # 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(4): + for port in ports: + try: + # connect and send a ping command + ser = serial.Serial(port, 115200, timeout=1) + #print(f"Checking port {port}...") + ser.write(b"ping\n") + response = ser.read_until("\n") + + # 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}!") + break + except: + pass + if self.port is not None: + break + + if self.port is None: + self.get_logger.info("Unable to find MCU... please make sure it is connected.") + time.sleep(1) + sys.exit(1) + + self.ser = serial.Serial(self.port, 115200) + atexit.register(self.cleanup) def run(self): global thread thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True) thread.start() - try: - while rclpy.ok(): - if self.ser.in_waiting: - self.read_mcu() - else: - time.sleep(0.1) - except KeyboardInterrupt: - pass - finally: - self.cleanup() + #if in arm mode, will need to read from the MCU + if self.launch_mode == 'arm': + try: + while rclpy.ok(): + if self.ser.in_waiting: + self.read_mcu() + else: + time.sleep(0.1) + except KeyboardInterrupt: + pass + finally: + self.cleanup() #Currently will just spit out all values over the /arm/feedback/debug topic as strings @@ -94,17 +94,17 @@ class SerialRelay(Node): try: output = str(self.ser.readline(), "utf8") if output: - print(f"[MCU] {output}", end="") + self.get_logger.info(f"[MCU] {output}", end="") msg = String() msg.data = output self.debug_pub.publish(msg) except serial.SerialException: - print("SerialException caught... closing serial port.") + self.get_logger.info("SerialException caught... closing serial port.") if self.ser.is_open: self.ser.close() pass except TypeError as e: - print(f"TypeError: {e}") + self.get_logger.info(f"TypeError: {e}") print("Closing serial port.") if self.ser.is_open: self.ser.close() @@ -126,8 +126,9 @@ class SerialRelay(Node): axis3 = msg.axis3 #Send controls for arm command = "can_relay_tovic,arm,40," + str(axis0) + "," + str(axis1) + "," + str(axis2) + "," + str(axis3) + "\n" - self.ser.write(bytes(command, "utf8")) - print(f"[Wrote] {command}", end="") + + self.send_cmd(command) + #print(f"[Wrote] {command}", end="") #Not yet finished, needs embedded implementation for new commands # ef_roll = msg.effector_roll @@ -141,110 +142,17 @@ class SerialRelay(Node): return + def send_cmd(self, cmd): + if self.launch_mode == 'anchor': #if in anchor mode, send to anchor node to relay + self.anchor_pub.publish(cmd) + elif self.launch_mode == 'arm': #if in standalone mode, send to MCU directly + self.ser.write(bytes(cmd, "utf8")) + #print(f"[Arm Wrote] {cmd}", end="") - # Depricated functions, kept temporarily for reference + def anchor_feedback(self, msg): + self.get_logger.info(f"[Anchor] {msg.data}", end="") + #self.send_cmd(msg.data) - # def send_controls(self, msg): - # command = "" - # ef_cmd = "" - # if(msg.b): - # command = "arm,stop\n" - # self.ser.write(bytes(command, "utf8")) - # print(f"[Wrote] {command}", end="") - # return - - # if(msg.a): - # command = "arm,endEffect,act,0"#retract actuator out - # self.ser.write(bytes(command, "utf8")) - # print(f"[Wrote] {command}", end="") - # elif(msg.x): - # command = "arm,endEffect,act,1"#extend actuator in - # self.ser.write(bytes(command, "utf8")) - # print(f"[Wrote] {command}", end="") - - # if(msg.plus): - # command = "arm,endEffect,laser,1\n" - # self.ser.write(bytes(command, "utf8")) - # print(f"[Wrote] {command}", end="") - # elif(msg.minus): - # command = "arm,endEffect,laser,0\n" - # self.ser.write(bytes(command, "utf8")) - # print(f"[Wrote] {command}", end="") - - # if(msg.rb): - # ef_cmd = "arm,endEffect,ctrl," - # if(msg.lt >= 0.5): - # ef_cmd += "-1," - # elif(msg.rt >= 0.5): - # ef_cmd += "1," - # else: - # ef_cmd += "0," - - # if(msg.rs_x < 0): - # ef_cmd += "-1," - # elif(msg.rs_x > 0): - # ef_cmd += "1," - # else: - # ef_cmd += "0," - # if(msg.ls_x < 0): - # ef_cmd += "-1" - # elif(msg.ls_x > 0): - # ef_cmd += "1" - # else: - # ef_cmd += "0" - - # command = ef_cmd + "\n" - # self.ser.write(bytes(command, "utf8")) - # print(f"[Wrote] {command}", end="") - # return - # else: - # ef_cmd = "arm,endEffect,ctrl," - # if(msg.lt >= 0.5): - # ef_cmd += "-1,0,0" - # elif(msg.rt >= 0.5): - # ef_cmd += "1,0,0" - # else: - # ef_cmd += "0,0,0" - # command = ef_cmd + "\n" - # self.ser.write(bytes(command, "utf8")) - # print(f"[Wrote] {command}", end="") - - # if(msg.lb): - # #self.ser.write(bytes("arm,setMode,manual\n", "utf8")) - # command = "arm,man,0.25," - # else: - # #self.ser.write(bytes("arm,setMode,manual\n", "utf8")) - # command = "arm,man,0.15," - - # if(msg.d_left): - # command += "-1," - # elif(msg.d_right): - # command += "1," - # else: - # command += "0," - # if(msg.ls_x < -0.4): - # command += "1," - # elif(msg.ls_x > 0.4): - # command += "-1," - # else: - # command += "0," - # if(msg.ls_y < -0.4): - # command += "1," - # elif(msg.ls_y > 0.4): - # command += "-1," - # else: - # command += "0," - # if(msg.rs_y < -0.4): - # command += "1" - # elif(msg.rs_y > 0.4): - # command += "-1" - # else: - # command += "0" - - # command += "\n" - # self.ser.write(bytes(command, "utf8")) - # print(f"[Wrote] {command}", end="") - # return @staticmethod def list_serial_ports(): diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 34e0ba4..b93e764 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -23,41 +23,50 @@ class SerialRelay(Node): # Initalize node with name super().__init__("core_node")#previously 'serial_publisher' - # Create publishers for feedback and telemetry + # Get launch mode parameter + self.declare_parameter('launch_mode', 'core') + launch_mode = self.get_parameter('launch_mode').value + self.get_logger().info(f"core launch_mode is: {launch_mode}") + + # Create publishers self.debug_pub = self.create_publisher(String, '/core/debug', 10) self.feedback_pub = self.create_publisher(CoreFeedback, '/core/feedback', 10) - - # Create a subscriber to listen to any commands sent for the MCU + # Create a subscriber self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, 10) - # Create a service server for pinging the rover self.ping_service = self.create_service(Empty, '/astra/core/ping', self.ping_callback) - # 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) - print(f"Checking port {port}...") - ser.write(b"ping\n") - response = ser.read_until("\n") + if launch_mode == 'anchor': + 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 pong is in response, then we are talking with the MCU - if b"pong" in response: - self.port = port - print(f"Found MCU at {self.port}!") - break - except: - pass - if self.port is not None: - break + + if 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") + + # 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}!") + break + except: + pass + if self.port is not None: + break if self.port is None: - print("Unable to find MCU...") + self.get_logger.info("Unable to find MCU...") time.sleep(1) sys.exit(1) @@ -71,11 +80,12 @@ class SerialRelay(Node): thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True) thread.start() - try: - while rclpy.ok(): - self.read_MCU() # Check the MCU for updates - except KeyboardInterrupt: - sys.exit(0) + if self.launch_mode == 'core': + try: + while rclpy.ok(): + self.read_MCU() # Check the MCU for updates + except KeyboardInterrupt: + sys.exit(0) def read_MCU(self): try: @@ -157,10 +167,18 @@ class SerialRelay(Node): command = "can_relay_tovic,core,19," + self.scale_duty(left_stick_neg, msg.max_speed) + ',' + self.scale_duty(msg.right_stick, msg.max_speed) + '\n' #print(f"[Sys] {command}", end="") - self.ser.write(bytes(command, "utf8"))# Send command to MCU - self.get_logger().debug(f"wrote: {command}") + #self.ser.write(bytes(command, "utf8"))# Send command to MCU + #self.get_logger().debug(f"wrote: {command}") + + self.send_cmd(command) + #print(f"[Sys] Relaying: {command}") + def send_cmd(self, cmd): + if self.launch_mode == 'anchor': + self.anchor_pub.publish(cmd) + elif self.launch_mode == 'core': + self.ser.write(bytes(cmd, "utf8")) def ping_callback(self, request, response): return response From 095b97000dbda15a6ece63c9f450c2aee694cd0e Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 11:22:20 -0600 Subject: [PATCH 04/33] Created anchor node, Updated .gitignore --- .gitignore | 3 + src/anchor_pkg/anchor_pkg/anchor_node.py | 141 +++++++++++++++++++++++ src/anchor_pkg/setup.py | 5 +- 3 files changed, 147 insertions(+), 2 deletions(-) create mode 100644 src/anchor_pkg/anchor_pkg/anchor_node.py diff --git a/.gitignore b/.gitignore index 69b791c..960675a 100644 --- a/.gitignore +++ b/.gitignore @@ -8,3 +8,6 @@ log/ #VSCode settings .vscode/ + +#Pycache folder +__pycache__/ \ No newline at end of file diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py new file mode 100644 index 0000000..31d8353 --- /dev/null +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -0,0 +1,141 @@ +import rclpy +from rclpy.node import Node +from std_srvs.srv import Empty + +import signal +import time +import atexit + +import serial +import sys +import threading +import glob + +from std_msgs.msg import String +from ros2_interfaces_pkg.msg import CoreFeedback +from ros2_interfaces_pkg.msg import CoreControl + +serial_pub = None +thread = None + +class SerialRelay(Node): + def __init__(self): + # Initalize node with name + super().__init__("anchor_node")#previously 'serial_publisher' + + + # Create publishers + self.arm_pub = self.create_publisher(String, '/anchor/arm/feedback', 10) + self.core_pub = self.create_publisher(String, '/anchor/core/feedback', 10) + self.bio_pub = self.create_publisher(String, '/anchor/bio/feedback', 10) + + self.debug_pub = self.create_publisher(String, '/anchor/debug', 10) + + # Create a subscriber + self.relay_sub = self.create_subscription(String, '/anchor/relay', self.send_cmd, 10) + + # 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(4): + 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") + + # 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}!") + 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) + + + 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(): + self.read_MCU() # Check the MCU for updates + except KeyboardInterrupt: + sys.exit(0) + + def read_MCU(self): + try: + output = str(self.ser.readline(), "utf8") + + if output: + # All output over debug temporarily + self.get_logger().info(f"[MCU] {output}", end="") + 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() + self.exit(1) + except TypeError as e: + print(f"TypeError: {e}") + print("Closing serial port.") + if self.ser.is_open: + self.ser.close() + self.exit(1) + except Exception as e: + print(f"Exception: {e}") + print("Closing serial port.") + if self.ser.is_open: + self.ser.close() + self.exit(1) + + def send_cmd(self, msg): + self.ser.write(bytes(msg, "utf8")) + + @staticmethod + def list_serial_ports(): + return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*") + + def cleanup(self): + print("Cleaning up before terminating...") + if self.ser.is_open: + self.ser.close() + +def myexcepthook(type, value, tb): + print("Uncaught exception:", type, value) + if serial_pub: + serial_pub.cleanup() + + +def main(args=None): + rclpy.init(args=args) + sys.excepthook = myexcepthook + + global serial_pub + + serial_pub = SerialRelay() + serial_pub.run() + +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() + diff --git a/src/anchor_pkg/setup.py b/src/anchor_pkg/setup.py index 376e0a1..e2844e7 100644 --- a/src/anchor_pkg/setup.py +++ b/src/anchor_pkg/setup.py @@ -15,10 +15,11 @@ setup( zip_safe=True, maintainer='tristan', maintainer_email='tristanmcginnis26@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', + description='Anchor node used to run all modules through a single modules MCU/Computer. Commands to all modules will be relayed through CAN', + license='All Rights Reserved', entry_points={ 'console_scripts': [ + "anchor = anchor_pkg.anchor_node:main" ], }, ) From 8e757464d11b2c818188e69928589a95624286d9 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 11:41:42 -0600 Subject: [PATCH 05/33] Fixed Core Node still trying to use ports in anchor mode --- .../__pycache__/rover_launch.cpython-312.pyc | Bin 1911 -> 0 bytes src/anchor_pkg/anchor_pkg/anchor_node.py | 4 +-- src/arm_pkg/arm_pkg/arm_node.py | 10 ++++---- src/core_pkg/core_pkg/core_node.py | 23 ++++++++++-------- 4 files changed, 20 insertions(+), 17 deletions(-) delete mode 100644 launch/__pycache__/rover_launch.cpython-312.pyc diff --git a/launch/__pycache__/rover_launch.cpython-312.pyc b/launch/__pycache__/rover_launch.cpython-312.pyc deleted file mode 100644 index a7099b21b0a6e223d8191f3ba9d5e378739c4565..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1911 zcmbtV&2Jk;6rc5e+Pik+B<=@p;!T?mEG2FcsgO{MfCeN~V-P_cEQGc3P8>J(?lQZZ zrd1;Oz=8Hadn;$eAt*WXPjGQ*C03(UL~1YGEF3O9@n&~zB|=duF{_<@GxO%Rzc>4P zZ+}atQwZ8mKm6LLY6!hxgF#EgMt=#6$H+t`*FtTs%5lt2v=VK;%D0885RUnl*p{kN zXbUa5tyC4T#qUrxX-d$i?#e~wF9uoUXtr$lR()*=yLHlZyryjx6EwGk>n(%eXzc^i z@Y~q(Xy!A=_}0fC!-mk2qhpcBP1{;+Hhf~l(?Z2Iah;1aq4BUMRvyYKCTr;`d z*uKp($K!1a4OX}L2HF<3cr&s7CPHLwOSml__hlnb9?Y4%DZnL~(iY$38r+_|8~YCs zIdm$9VkY7Ge;6q<0oN%Q%LpPl@QFpH+5SFxEQPat;?$w5c?m8N~~#wXT> z(Q2AHW2HO9-e{WGEa^AbY}>`UZNa_zt`6kpb%r*t!>)Nff-Dt9A~7i0!DQ8D0T_;h zEt4>vQPCky%VPo5ZOg-(9u*)UmrCn**pskhqM`*ac3mbuowZlzOIM=kuK*cLYSEzN z+@)H4DlYbXhv=|Ff!qBM5~PD(s*^u%JXL4^P)kqL($1Zy>N}mKK+AMKI*`@xH-DGU z9VqFC+I?+b8wt)&1$jNly%C%%1QW%b#qPzc-TYgJs+7(g@=7w*8%4>?(D_o3p9*sM z6P>4zI!_1r%Rz49MCY$Lq-8th-YpKLGTrRlljPiidgjr!2R8yO`>=e!ygxD?T$~9C z)4_#FhBdXY<8}4LZs8j6%Z>uSbf&iyqr8nWqmRCN@XgV^vq51dxKKE8@70jNp$M$I znNOc7l>>F8Q|`l2qB`pajeS{GbNXLGcf2M+#`1f@Zd{SJkvFVOTgV&M9giV|ZBHEJ zK_fA+ARDAgr~$&N-@Vy#sQFNc{}r4~V$DJMO`CYt@LOIDl7buzF z>|`*0{ZJOhq|U8F4QUgBGPbXr{aJgaEc8TvQtBgKlY594WikaDD@Bz`k&HuLG!x6) z2!9!F#F&w#4@@f^qfBOz|)PxStCwD=rd{#*E*Vo0{{q{p&t(7r diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 31d8353..ff42ec7 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -49,7 +49,7 @@ class SerialRelay(Node): # 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"Found MCU at {self.port}!") break except: pass @@ -57,7 +57,7 @@ class SerialRelay(Node): break if self.port is None: - self.get_logger.info("Unable to find MCU...") + self.get_logger().info("Unable to find MCU...") time.sleep(1) sys.exit(1) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 8d70e06..abf82e9 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -22,8 +22,8 @@ class SerialRelay(Node): # Get launch mode parameter self.declare_parameter('launch_mode', 'arm') - launch_mode = self.get_parameter('launch_mode').value - self.get_logger().info(f"arm launch_mode is: {launch_mode}") + self.launch_mode = self.get_parameter('launch_mode').value + self.get_logger().info(f"arm launch_mode is: {self.launch_mode}") # Create publishers 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) # 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_pub = self.create_publisher(String, '/anchor/relay', 10) # 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 self.port = None ports = SerialRelay.list_serial_ports() @@ -150,7 +150,7 @@ class SerialRelay(Node): #print(f"[Arm Wrote] {cmd}", end="") 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) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index b93e764..e2cea5d 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -25,8 +25,8 @@ class SerialRelay(Node): # Get launch mode parameter self.declare_parameter('launch_mode', 'core') - launch_mode = self.get_parameter('launch_mode').value - self.get_logger().info(f"core launch_mode is: {launch_mode}") + self.launch_mode = self.get_parameter('launch_mode').value + self.get_logger().info(f"core launch_mode is: {self.launch_mode}") # Create publishers 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 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_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 self.port = None ports = SerialRelay.list_serial_ports() @@ -65,13 +65,13 @@ class SerialRelay(Node): 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) + 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) + self.ser = serial.Serial(self.port, 115200) + atexit.register(self.cleanup) def run(self): @@ -180,6 +180,9 @@ class SerialRelay(Node): elif self.launch_mode == 'core': 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): return response From af89407aa47b1b5fb8f22bd9819c38990ae9e82c Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 11:44:13 -0600 Subject: [PATCH 06/33] Update launch file to output to screen and log file --- launch/rover_launch.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/launch/rover_launch.py b/launch/rover_launch.py index 95bf004..e4ec2ff 100644 --- a/launch/rover_launch.py +++ b/launch/rover_launch.py @@ -17,7 +17,7 @@ def launch_setup(context, *args, **kwargs): package='arm_pkg', executable='arm', # change as needed name='arm', - output='screen', + output='both', parameters=[{'launch_mode': mode}] ) ) @@ -26,7 +26,7 @@ def launch_setup(context, *args, **kwargs): package='core_pkg', executable='core', # change as needed name='core', - output='screen', + output='both', parameters=[{'launch_mode': mode}] ) ) @@ -35,7 +35,7 @@ def launch_setup(context, *args, **kwargs): # package='bio_pkg', # executable='bio', # change as needed # name='bio', - # output='screen', + # output='both', # parameters=[{'launch_mode': mode}] # ) # ) @@ -44,7 +44,7 @@ def launch_setup(context, *args, **kwargs): package='anchor_pkg', executable='anchor', # change as needed name='anchor', - output='screen', + output='both', parameters=[{'launch_mode': mode}] ) ) @@ -56,7 +56,7 @@ def launch_setup(context, *args, **kwargs): package='arm_pkg', executable='arm', name='arm', - output='screen', + output='both', parameters=[{'launch_mode': mode}] ) ) @@ -66,7 +66,7 @@ def launch_setup(context, *args, **kwargs): package='core_pkg', executable='core', name='core', - output='screen', + output='both', parameters=[{'launch_mode': mode}] ) ) @@ -76,7 +76,7 @@ def launch_setup(context, *args, **kwargs): # package='bio_pkg', # executable='bio', # name='bio', - # output='screen', + # output='both', # parameters=[{'launch_mode': mode}] # ) # ) From 0c03c139d9b8ad3fba8447c5ca87ca26fdef8d90 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 11:45:57 -0600 Subject: [PATCH 07/33] Move launch file out of subfolder --- launch/rover_launch.py => rover_launch.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename launch/rover_launch.py => rover_launch.py (100%) diff --git a/launch/rover_launch.py b/rover_launch.py similarity index 100% rename from launch/rover_launch.py rename to rover_launch.py From d373622c7274916119756feb70527064d02762cc Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 12:33:15 -0600 Subject: [PATCH 08/33] update README.md --- README.md | 144 ++++++++++++++++++++++++++++-------------------------- 1 file changed, 76 insertions(+), 68 deletions(-) diff --git a/README.md b/README.md index 6925ce2..ec6562f 100644 --- a/README.md +++ b/README.md @@ -1,83 +1,91 @@ # rover-ros2 -Submodule which includes all ros2 packages for the rover. These are centrally located for modular rover operation. You can use launch files located here to run any module as standalone or as a full-rover relay. +Submodule which includes all ros2 packages for the rover. These are centrally located for modular rover operation. + +You will use this package to launch any module-side ROS2 nodes. +
+#### Table of Contents + +\- \[Software Prerequisites\] \(\#Software\-Pre\-reqs\) + +
## Software Pre-reqs +An acting base station computer will need several things: -The BaseStation computer will need several things. +* ROS2 Humble + * Follow the standard ROS2 humble install process. Linux recommended. + * https://docs.ros.org/en/humble/Installation.html +* Colcon + * `$ sudo apt update` + * `$ sudo apt install python3-colcon-common-extensions` +* Configured Static IP for Ubiquiti bullet (Process varies by OS) + * IP Address: 192.168.1.x + * This can be just about anything not already in use. I recommend something 30-39 + * Net Mask: 255.255.255.0 + * Gateway: 192.168.1.0 -- ROS2 Humble +## Launching with ANCHOR -- Follow the standard ROS2 humble install process. Linux recommended. +ANCHOR (Active Node Controller Hub and Operational Relay) +Allows for launching all nodes on the rover simulataneously. Additionally, all controls will run through the core's NUC and MCU. +
+1. SSH to core + * Core1: `ssh clucky@192.168.1.69` + * Core2: `ssh clucky@192.168.1.70` + * Password: \ +2. Navigate to rover-ros2 workspace + * `cd rover-ros2` +3. Source the workspace + * `source install/setup.bash` +4. Launch ANCHOR + * `ros2 launch rover_launch.py mode:=anchor` -- https://docs.ros.org/en/humble/Installation.html +## Launching as Standalone -- Colcon +For use when running independent modules through their respective computers (pi/NUC) without ANCHOR. -- `$ sudo apt update` +1. SSH to the the module's computer + * Core1: `ssh clucky@192.168.1.69` + * Core2: `ssh clucky@192.168.1.70` + * Arm: `ssh arm@192.168.1.70` + * Bio: \ + * Password: \ +2. Run the main node (this sends commands to the MCU) + * Navigate to the rover-ros2 workspace (location may vary) + * `cd rover-ros2` + * Source the workspace + * `source install/setup.bash` + * Start the node + * ARM: `ros2 launch rover_launch.py mode:=arm` + * CORE: `ros2 launch rover_launch.py mode:=core` + * BIO: `ros2 launch rover_launch.py mode:=bio` -- `$ sudo apt install python3-colcon-common-extensions` +## Running Headless -- Configured Static IP for Ubiquiti bullet +Headless control nodes (for ARM and CORE) allow running of the module on-rover without the operator having ROS2 installed on their machine. You will need a laptop to connect to the pi/NUC in order to launch headless but it can be disconnected after the nodes are spun up. +
+1. SSH to the the module's computer + * Core1: `ssh clucky@192.168.1.69` + * Core2: `ssh clucky@192.168.1.70` + * Arm: `ssh arm@192.168.1.70` + * Password: \ +2. Run the  headless node + * You must have ANCHOR or the module's Standalone node running + * Open a new terminal (SSH'd to the module) + * Navigate to rover-ros2 workspace + * `cd rover-ros2` + * Source the workspace + * `source install/setup.bash` + * Run the node (ensure controller is connected and on x-input mode) + * CORE: `ros2 run core_pkg headless` + * ARM: `ros2 run arm_pkg headless` -- This process will depend on the system, but you'll need this for using the Ubiquiti network with the M2 bullets. - -- Settings: - -- Net Mask: 255.255.255.0 - -- IP Address: 192.168.1.x - -- This can be just about anything, just not ending in 1.20, 1.21, 1.69, or 1.0 - -- Gateway: 192.168.1.0 - - -## Headless Control - - To control the rover without a base station you'll need to run the headless control node. - -- SSH to the the rover from a computer computer over wifi (or connect directly) - -- `ssh clucky@192.168.1.69` - -- Password: spaceiscool639 - -- Run `Ros2 run headless_pkg headless` - - This needs to be done in a separate ssh/shell window from the core rover node - - - -## Running the Rover node - - -- Ensure you're either connected directly to the rover's network switch over ethernet or connected to an M2 bullet which has connection to the rover (red signal lights on) - -- SSH to the the rover from a computer computer over wifi (or connect directly) - -- `ssh clucky@192.168.1.69` - -- Password: spaceiscool639 - -- run `ros2 run core_control_pkg core_control` - - This needs to be done in a separate ssh/shell window from the headless control node (if you're running it) - -- You should see "MCU found" on a serial line - -- If not, you'll get an error it's not found and the program will close. Ensure the teensy is connected via usb to the Nuc - - - -## Connecting the GuliKit Controller (Recommended) - - - -- Connect controller to pc with USB-C - -- Select the "X-Input" control mode (Windows logo) on the controller. - -- Hold the button next to the symbols (windows, android, switch, etc...) - -- You'll need to release the button and press down again to cycle to the next mode +## Connecting the GuliKit Controller +Connecting the GuliKit Controller (Recommended) +* Connect controller to pc with USB-C +* Select the "X-Input" control mode (Windows logo) on the controller. +* Hold the button next to the symbols (windows, android, switch, etc...) +* You'll need to release the button and press down again to cycle to the next mode \ No newline at end of file From 62ef39547115400c3c3899ef5b375f096e1b59cc Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 12:35:55 -0600 Subject: [PATCH 09/33] Add hyperlinks to README --- README.md | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index ec6562f..d1d4cac 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,12 @@ You will use this package to launch any module-side ROS2 nodes.
#### Table of Contents -\- \[Software Prerequisites\] \(\#Software\-Pre\-reqs\) +- [Software Prerequisites](#Software-Pre-reqs) +- [Launching with ANCHOR](#Launching-with-ANCHOR) +- [Launching as Standalone](#Launching-as-Standalone) +- [Running Headless](#Running-Headless) +- [Connecting the GuliKit Controller](#Connecting-the-GuliKit-Contoller) +
## Software Pre-reqs From 46682b2a3be69987acf93c755606b58011755c64 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 12:36:36 -0600 Subject: [PATCH 10/33] update readme --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index d1d4cac..529737e 100644 --- a/README.md +++ b/README.md @@ -6,11 +6,11 @@ You will use this package to launch any module-side ROS2 nodes.
#### Table of Contents -- [Software Prerequisites](#Software-Pre-reqs) -- [Launching with ANCHOR](#Launching-with-ANCHOR) -- [Launching as Standalone](#Launching-as-Standalone) -- [Running Headless](#Running-Headless) -- [Connecting the GuliKit Controller](#Connecting-the-GuliKit-Contoller) +- [Software Prerequisites](##Software-Pre-reqs) +- [Launching with ANCHOR](##Launching-with-ANCHOR) +- [Launching as Standalone](##Launching-as-Standalone) +- [Running Headless](##Running-Headless) +- [Connecting the GuliKit Controller](##Connecting-the-GuliKit-Contoller)
From 50c83cceecae916a2d90207c316e259c61c4cf98 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 12:37:42 -0600 Subject: [PATCH 11/33] final fix to README formatting --- README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/README.md b/README.md index 529737e..31cde60 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,6 @@ You will use this package to launch any module-side ROS2 nodes. - [Connecting the GuliKit Controller](##Connecting-the-GuliKit-Contoller) -
## Software Pre-reqs An acting base station computer will need several things: From 46c63ade8c700cfc8f0bd76578d574aafb15ac4a Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 12:38:29 -0600 Subject: [PATCH 12/33] Final readme update. Remove TOC --- README.md | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/README.md b/README.md index 31cde60..fceca4f 100644 --- a/README.md +++ b/README.md @@ -4,14 +4,6 @@ Submodule which includes all ros2 packages for the rover. These are centrally lo You will use this package to launch any module-side ROS2 nodes.
-#### Table of Contents - -- [Software Prerequisites](##Software-Pre-reqs) -- [Launching with ANCHOR](##Launching-with-ANCHOR) -- [Launching as Standalone](##Launching-as-Standalone) -- [Running Headless](##Running-Headless) -- [Connecting the GuliKit Controller](##Connecting-the-GuliKit-Contoller) - ## Software Pre-reqs @@ -92,4 +84,4 @@ Connecting the GuliKit Controller (Recommended) * Connect controller to pc with USB-C * Select the "X-Input" control mode (Windows logo) on the controller. * Hold the button next to the symbols (windows, android, switch, etc...) -* You'll need to release the button and press down again to cycle to the next mode \ No newline at end of file +* You'll need to release the button and press down again to cycle to the next mode From eb1effd7910b058a79594b955deddd54299c2e31 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 14:10:27 -0600 Subject: [PATCH 13/33] fix anchor crashing --- src/anchor_pkg/anchor_pkg/anchor_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index ff42ec7..574c5cb 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -83,7 +83,7 @@ class SerialRelay(Node): if output: # All output over debug temporarily - self.get_logger().info(f"[MCU] {output}", end="") + self.get_logger().info(f"[MCU] {output}") msg = String() msg.data = output self.debug_pub.publish(msg) From aa231d43ca9c6b2da88eaaaf1aa9be7794299e38 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 14:16:11 -0600 Subject: [PATCH 14/33] core debug outputs --- src/core_pkg/core_pkg/core_node.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index e2cea5d..4ee80fb 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -176,8 +176,10 @@ class SerialRelay(Node): def send_cmd(self, cmd): if self.launch_mode == 'anchor': + self.get_logger().info(f"[Core to Anchor Relay] {cmd}") self.anchor_pub.publish(cmd) elif self.launch_mode == 'core': + self.get_logger().info(f"[Core to MCU] {cmd}") self.ser.write(bytes(cmd, "utf8")) def anchor_feedback(self, msg): From 6f98eb231a19e8bcdd1ef81e97d07a372d4c814d Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 14:17:45 -0600 Subject: [PATCH 15/33] potential fix for core node not spinning in anchor mode --- src/core_pkg/core_pkg/core_node.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 4ee80fb..c170c94 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -80,12 +80,13 @@ class SerialRelay(Node): thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True) thread.start() - if self.launch_mode == 'core': - try: - while rclpy.ok(): + + try: + while rclpy.ok(): + if self.launch_mode == 'core': self.read_MCU() # Check the MCU for updates - except KeyboardInterrupt: - sys.exit(0) + except KeyboardInterrupt: + sys.exit(0) def read_MCU(self): try: From b2bb4c1e67b0b56d92e1f5e47ca39177a0ef468f Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 14:20:31 -0600 Subject: [PATCH 16/33] fix: control msg for anchor should relay msg.data --- src/core_pkg/core_pkg/core_node.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index c170c94..931ce9e 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -175,13 +175,13 @@ class SerialRelay(Node): #print(f"[Sys] Relaying: {command}") - def send_cmd(self, cmd): + def send_cmd(self, msg): if self.launch_mode == 'anchor': - self.get_logger().info(f"[Core to Anchor Relay] {cmd}") - self.anchor_pub.publish(cmd) + self.get_logger().info(f"[Core to Anchor Relay] {msg.data}") + self.anchor_pub.publish(msg.data) elif self.launch_mode == 'core': - self.get_logger().info(f"[Core to MCU] {cmd}") - self.ser.write(bytes(cmd, "utf8")) + self.get_logger().info(f"[Core to MCU] {msg.data}") + self.ser.write(bytes(msg.data, "utf8")) def anchor_feedback(self, msg): self.get_logger.info(f"[Arm Anchor] {msg.data}", end="") From 86ad80f8db8ee22587a37f496c5b09c7062a1ae8 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 14:24:02 -0600 Subject: [PATCH 17/33] more testing with msg.data --- src/core_pkg/core_pkg/core_node.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 931ce9e..0741e04 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -177,14 +177,14 @@ class SerialRelay(Node): def send_cmd(self, msg): if self.launch_mode == 'anchor': - self.get_logger().info(f"[Core to Anchor Relay] {msg.data}") + self.get_logger().info(f"[Core to Anchor Relay] {msg}") self.anchor_pub.publish(msg.data) elif self.launch_mode == 'core': - self.get_logger().info(f"[Core to MCU] {msg.data}") - self.ser.write(bytes(msg.data, "utf8")) + self.get_logger().info(f"[Core to MCU] {msg}") + self.ser.write(bytes(msg, "utf8")) def anchor_feedback(self, msg): - self.get_logger.info(f"[Arm Anchor] {msg.data}", end="") + self.get_logger.info(f"[Arm Anchor] {msg}") def ping_callback(self, request, response): return response From 251c87708339026d3b211e552840833a50923f7c Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 14:27:57 -0600 Subject: [PATCH 18/33] more testing --- src/core_pkg/core_pkg/core_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 0741e04..6f2aef1 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -178,7 +178,7 @@ class SerialRelay(Node): def send_cmd(self, msg): if self.launch_mode == 'anchor': self.get_logger().info(f"[Core to Anchor Relay] {msg}") - self.anchor_pub.publish(msg.data) + self.anchor_pub.publish(msg) elif self.launch_mode == 'core': self.get_logger().info(f"[Core to MCU] {msg}") self.ser.write(bytes(msg, "utf8")) From 93a24b6eab2d4bc430baa4bac2bc6c2b0520f275 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 14:31:53 -0600 Subject: [PATCH 19/33] final fix for string output on anchor_relay for core --- src/core_pkg/core_pkg/core_node.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 6f2aef1..1c92b71 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -178,7 +178,9 @@ class SerialRelay(Node): def send_cmd(self, msg): if self.launch_mode == 'anchor': self.get_logger().info(f"[Core to Anchor Relay] {msg}") - self.anchor_pub.publish(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")) From 10192c746e357f693eeb75a4e39eb76ccf8c8d7f Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 14:34:35 -0600 Subject: [PATCH 20/33] debug output on anchor --- src/anchor_pkg/anchor_pkg/anchor_node.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 574c5cb..ac5c450 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -108,6 +108,7 @@ class SerialRelay(Node): self.exit(1) def send_cmd(self, msg): + self.get_logger().info(f"Sending command to MCU: {msg}") self.ser.write(bytes(msg, "utf8")) @staticmethod From fe5ed1a071f52ec4dd8d75902ca153b52c9a743d Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 14:47:10 -0600 Subject: [PATCH 21/33] fix message type issues? --- src/anchor_pkg/anchor_pkg/anchor_node.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index ac5c450..9098775 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -108,8 +108,9 @@ class SerialRelay(Node): self.exit(1) def send_cmd(self, msg): + message = msg.data self.get_logger().info(f"Sending command to MCU: {msg}") - self.ser.write(bytes(msg, "utf8")) + self.ser.write(bytes(message, "utf8")) @staticmethod def list_serial_ports(): From d000a2007a1037984fa4e1332eeac1805ab02e5b Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 14:53:44 -0600 Subject: [PATCH 22/33] remove debug prints --- src/anchor_pkg/anchor_pkg/anchor_node.py | 2 +- src/core_pkg/core_pkg/core_node.py | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 9098775..859fff0 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -109,7 +109,7 @@ class SerialRelay(Node): def send_cmd(self, msg): message = msg.data - self.get_logger().info(f"Sending command to MCU: {msg}") + #self.get_logger().info(f"Sending command to MCU: {msg}") self.ser.write(bytes(message, "utf8")) @staticmethod diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 1c92b71..d900b1b 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -174,10 +174,9 @@ class SerialRelay(Node): self.send_cmd(command) #print(f"[Sys] Relaying: {command}") - def send_cmd(self, msg): if self.launch_mode == 'anchor': - self.get_logger().info(f"[Core to Anchor Relay] {msg}") + #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) From 4090f63890121b6e8c99cff16100ec5c39d94039 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 15:00:17 -0600 Subject: [PATCH 23/33] Update arm node message types for anchor --- src/arm_pkg/arm_pkg/arm_node.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index abf82e9..9333a19 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -142,12 +142,13 @@ class SerialRelay(Node): return - def send_cmd(self, cmd): + def send_cmd(self, msg): if self.launch_mode == 'anchor': #if in anchor mode, send to anchor node to relay - self.anchor_pub.publish(cmd) + output = String() + output.data = msg + self.anchor_pub.publish(output) elif self.launch_mode == 'arm': #if in standalone mode, send to MCU directly - self.ser.write(bytes(cmd, "utf8")) - #print(f"[Arm Wrote] {cmd}", end="") + self.ser.write(bytes(msg, "utf8")) def anchor_feedback(self, msg): self.get_logger.info(f"[Arm Anchor] {msg.data}", end="") From 3f3b0f43025eb1fbd1b3b5fe51dc932324a95d49 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 15:25:18 -0600 Subject: [PATCH 24/33] fix arm not spinning --- src/arm_pkg/arm_pkg/arm_node.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 9333a19..92bb9a2 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -76,17 +76,18 @@ class SerialRelay(Node): thread.start() #if in arm mode, will need to read from the MCU - if self.launch_mode == 'arm': - try: - while rclpy.ok(): + + try: + while rclpy.ok(): + if self.launch_mode == 'arm': if self.ser.in_waiting: self.read_mcu() else: time.sleep(0.1) - except KeyboardInterrupt: - pass - finally: - self.cleanup() + except KeyboardInterrupt: + pass + finally: + self.cleanup() #Currently will just spit out all values over the /arm/feedback/debug topic as strings From a754a270c1edb5143356182d85f82e8dffa4b33e Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 23:18:48 -0600 Subject: [PATCH 25/33] fix get_logger() calls --- src/arm_pkg/arm_pkg/arm_node.py | 12 ++++++------ src/core_pkg/core_pkg/core_node.py | 6 +++--- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 92bb9a2..23b5b7e 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -55,7 +55,7 @@ class SerialRelay(Node): # 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"Found MCU at {self.port}!") break except: pass @@ -63,7 +63,7 @@ class SerialRelay(Node): break if self.port is None: - self.get_logger.info("Unable to find MCU... please make sure it is connected.") + self.get_logger().info("Unable to find MCU... please make sure it is connected.") time.sleep(1) sys.exit(1) @@ -95,17 +95,17 @@ class SerialRelay(Node): try: output = str(self.ser.readline(), "utf8") if output: - self.get_logger.info(f"[MCU] {output}", end="") + self.get_logger().info(f"[MCU] {output}", end="") msg = String() msg.data = output self.debug_pub.publish(msg) except serial.SerialException: - self.get_logger.info("SerialException caught... closing serial port.") + self.get_logger().info("SerialException caught... closing serial port.") if self.ser.is_open: self.ser.close() pass except TypeError as e: - self.get_logger.info(f"TypeError: {e}") + self.get_logger().info(f"TypeError: {e}") print("Closing serial port.") if self.ser.is_open: self.ser.close() @@ -152,7 +152,7 @@ class SerialRelay(Node): self.ser.write(bytes(msg, "utf8")) def anchor_feedback(self, msg): - self.get_logger.info(f"[Arm Anchor] {msg.data}", end="") + self.get_logger().info(f"[Arm Anchor] {msg.data}", end="") #self.send_cmd(msg.data) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index d900b1b..d4bbb2f 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -58,7 +58,7 @@ class SerialRelay(Node): # 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"Found MCU at {self.port}!") break except: pass @@ -66,7 +66,7 @@ class SerialRelay(Node): break if self.port is None: - self.get_logger.info("Unable to find MCU...") + self.get_logger().info("Unable to find MCU...") time.sleep(1) sys.exit(1) @@ -185,7 +185,7 @@ class SerialRelay(Node): self.ser.write(bytes(msg, "utf8")) def anchor_feedback(self, msg): - self.get_logger.info(f"[Arm Anchor] {msg}") + self.get_logger().info(f"[Arm Anchor] {msg}") def ping_callback(self, request, response): return response From 2d03a45e404891008f710d639a862ec2307b3d19 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 20 Feb 2025 23:20:22 -0600 Subject: [PATCH 26/33] fix up prints --- src/arm_pkg/arm_pkg/arm_node.py | 4 ++-- src/core_pkg/core_pkg/core_node.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 23b5b7e..e5c2c64 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -95,7 +95,7 @@ class SerialRelay(Node): try: output = str(self.ser.readline(), "utf8") if output: - self.get_logger().info(f"[MCU] {output}", end="") + self.get_logger().info(f"[MCU] {output}") msg = String() msg.data = output self.debug_pub.publish(msg) @@ -152,7 +152,7 @@ class SerialRelay(Node): self.ser.write(bytes(msg, "utf8")) def anchor_feedback(self, msg): - self.get_logger().info(f"[Arm Anchor] {msg.data}", end="") + self.get_logger().info(f"[Arm Anchor] {msg.data}") #self.send_cmd(msg.data) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index d4bbb2f..8b4aeff 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -94,7 +94,7 @@ class SerialRelay(Node): if output: # All output over debug temporarily - print(f"[MCU] {output}", end="") + print(f"[MCU] {output}") msg = String() msg.data = output self.debug_pub.publish(msg) From 6d67c60d51a4f3e2e9ad074c485e01451da41158 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 22 Feb 2025 01:07:53 -0600 Subject: [PATCH 27/33] headless collects data for effector controls --- src/arm_pkg/arm_pkg/arm_headless.py | 69 ++++++++++++++++++++++------- 1 file changed, 53 insertions(+), 16 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_headless.py b/src/arm_pkg/arm_pkg/arm_headless.py index c5f5721..737852f 100755 --- a/src/arm_pkg/arm_pkg/arm_headless.py +++ b/src/arm_pkg/arm_pkg/arm_headless.py @@ -45,8 +45,7 @@ class Headless(Node): self.debug_sub = self.create_subscription(String, '/arm/feedback/debug', self.read_feedback, 10) - #self.lastMsg = String() #Used to ignore sending controls repeatedly when they do not change - + self.laser_status = 0 # Initialize pygame pygame.init() @@ -106,23 +105,61 @@ class Headless(Node): exit() input = ArmManual() - dpad_input = self.gamepad.get_hat(0) - input.axis0 = 0 - if dpad_input[0] == 1: - input.axis0 = 1 - elif dpad_input[0] == -1: - input.axis0 = -1 - input.axis1 = -1 * round(self.gamepad.get_axis(0))#left x-axis - input.axis2 = -1 * round(self.gamepad.get_axis(1))#left y-axis - input.axis3 = -1 * round(self.gamepad.get_axis(4))#right y-axis + # Triggers for gripper control + if self.gamepad.get_axis(2) > 0:#left trigger + input.gripper = -1 + elif self.gamepad.get_axis(5) > 0:#right trigger + input.gripper = 1 + + # Toggle Laser + if self.gamepad.get_button(0):#Start + self.laser_status = 1 + elif self.gamepad.get_button(1):#Back + self.laser_status = 0 + input.laser = self.laser_status + + + if self.gamepad.get_button(5):#right bumper, control effector + + # Left stick X-axis for effector yaw + if self.gamepad.get_axis(0) > 0: + input.effeector_yaw = 1 + elif self.gamepad.get_axis(0) < 0: + input.effector_yaw = -1 + + # Right stick X-axis for effector roll + if self.gamepad.get_axis(3) > 0: + input.effector_roll = 1 + elif self.gamepad.get_axis(3) < 0: + input.effector_roll = -1 + + else: # Control arm axis + dpad_input = self.gamepad.get_hat(0) + input.axis0 = 0 + if dpad_input[0] == 1: + input.axis0 = 1 + elif dpad_input[0] == -1: + input.axis0 = -1 + + input.axis1 = -1 * round(self.gamepad.get_axis(0))#left x-axis + input.axis2 = -1 * round(self.gamepad.get_axis(1))#left y-axis + input.axis3 = -1 * round(self.gamepad.get_axis(4))#right y-axis + + + #Button Mappings + #axis2 -> LT + #axis5 -> RT + #Buttons0 -> A + #Buttons1 -> B + #Buttons2 -> X + #Buttons3 -> Y + #Buttons4 -> LB + #Buttons5 -> RB + #Buttons6 -> Back + #Buttons7 -> Start - #Temporary, not controlling digit. Awaiting embedded implementation - input.effector_yaw = 0 - input.effector_roll = 0 - input.gripper = 0 input.linear_actuator = 0 - input.laser = 0 if pygame.joystick.get_count() != 0: From 136fc01d737e2fe2459161ec1570065b01870817 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 22 Feb 2025 01:24:16 -0600 Subject: [PATCH 28/33] add effector CAN relay commands to arm_node --- src/arm_pkg/arm_pkg/arm_node.py | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index e5c2c64..e5a0ba8 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -125,10 +125,26 @@ class SerialRelay(Node): axis1 = msg.axis1 axis2 = msg.axis2 axis3 = msg.axis3 + #Send controls for arm command = "can_relay_tovic,arm,40," + str(axis0) + "," + str(axis1) + "," + str(axis2) + "," + str(axis3) + "\n" - self.send_cmd(command) + + #Send controls for end effector + command = "can_relay_tovic_digit,35," + str(msg.effector_roll) + "\n" + self.send_cmd(command) + + command = "can_relay_tovic_digit,36,0," + str(msg.effector_yaw) + "\n" + self.send_cmd(command) + + command = "can_relay_tovic_digit,26," + str(msg.gripper) + "\n" + self.send_cmd(command) + + command = "can_relay_tovic_digit,28," + str(msg.laser) + "\n" + self.send_cmd(command) + + + #print(f"[Wrote] {command}", end="") #Not yet finished, needs embedded implementation for new commands From a7546ece49e63fa4bf18653d01024fa62f3f3a2c Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 22 Feb 2025 01:25:58 -0600 Subject: [PATCH 29/33] nuke __pycache__ in future --- rover_launch.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/rover_launch.py b/rover_launch.py index e4ec2ff..e34a34d 100644 --- a/rover_launch.py +++ b/rover_launch.py @@ -5,6 +5,12 @@ from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node + + +#Prevent making __pycache__ directories +from sys import dont_write_bytecode +dont_write_bytecode = True + def launch_setup(context, *args, **kwargs): # Retrieve the resolved value of the launch argument 'mode' mode = LaunchConfiguration('mode').perform(context) From 87e93bc45e30733235d5984a908991c14d47e380 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 22 Feb 2025 02:35:05 -0600 Subject: [PATCH 30/33] fix crashing for misspell --- src/arm_pkg/arm_pkg/arm_headless.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_headless.py b/src/arm_pkg/arm_pkg/arm_headless.py index 737852f..88df1e6 100755 --- a/src/arm_pkg/arm_pkg/arm_headless.py +++ b/src/arm_pkg/arm_pkg/arm_headless.py @@ -113,18 +113,17 @@ class Headless(Node): input.gripper = 1 # Toggle Laser - if self.gamepad.get_button(0):#Start + if self.gamepad.get_button(7):#Start self.laser_status = 1 - elif self.gamepad.get_button(1):#Back + elif self.gamepad.get_button(6):#Back self.laser_status = 0 input.laser = self.laser_status - if self.gamepad.get_button(5):#right bumper, control effector # Left stick X-axis for effector yaw if self.gamepad.get_axis(0) > 0: - input.effeector_yaw = 1 + input.effector_yaw = 1 elif self.gamepad.get_axis(0) < 0: input.effector_yaw = -1 From 79da299e82481ec0d210da42c92d128aac70d5b7 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 22 Feb 2025 02:35:47 -0600 Subject: [PATCH 31/33] test output --- src/arm_pkg/arm_pkg/arm_node.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index e5a0ba8..8347e78 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -165,6 +165,7 @@ class SerialRelay(Node): output.data = msg self.anchor_pub.publish(output) elif self.launch_mode == 'arm': #if in standalone mode, send to MCU directly + self.get_logger().info(f"[Arm to MCU] {msg}") self.ser.write(bytes(msg, "utf8")) def anchor_feedback(self, msg): From 2e73fd9c8329d3435414a9df662979fe758c1d96 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 22 Feb 2025 02:52:51 -0600 Subject: [PATCH 32/33] fix CAN commands --- src/arm_pkg/arm_pkg/arm_node.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 8347e78..63c5a57 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -131,16 +131,16 @@ class SerialRelay(Node): self.send_cmd(command) #Send controls for end effector - command = "can_relay_tovic_digit,35," + str(msg.effector_roll) + "\n" + command = "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n" self.send_cmd(command) - command = "can_relay_tovic_digit,36,0," + str(msg.effector_yaw) + "\n" + command = "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n" self.send_cmd(command) - command = "can_relay_tovic_digit,26," + str(msg.gripper) + "\n" + command = "can_relay_tovic,digit,26," + str(msg.gripper) + "\n" self.send_cmd(command) - command = "can_relay_tovic_digit,28," + str(msg.laser) + "\n" + command = "can_relay_tovic,digit,28," + str(msg.laser) + "\n" self.send_cmd(command) From f456aa732c10f4bd09a107dc41c22f55cffefe2a Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sun, 23 Feb 2025 09:35:15 -0600 Subject: [PATCH 33/33] lower deadzone for arm headless --- src/arm_pkg/arm_pkg/arm_headless.py | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_headless.py b/src/arm_pkg/arm_pkg/arm_headless.py index 88df1e6..cbe0108 100755 --- a/src/arm_pkg/arm_pkg/arm_headless.py +++ b/src/arm_pkg/arm_pkg/arm_headless.py @@ -141,9 +141,18 @@ class Headless(Node): elif dpad_input[0] == -1: input.axis0 = -1 - input.axis1 = -1 * round(self.gamepad.get_axis(0))#left x-axis - input.axis2 = -1 * round(self.gamepad.get_axis(1))#left y-axis - input.axis3 = -1 * round(self.gamepad.get_axis(4))#right y-axis + if self.gamepad.get_axis(0) > .15 or self.gamepad.get_axis(0) < -.15: + input.axis1 = -1 * round(self.gamepad.get_axis(0)) + + if self.gamepad.get_axis(1) > .15 or self.gamepad.get_axis(1) < -.15: + input.axis2 = -1 * round(self.gamepad.get_axis(1)) + + if self.gamepad.get_axis(4) > .15 or self.gamepad.get_axis(4) < -.15: + input.axis3 = -1 * round(self.gamepad.get_axis(4)) + + # input.axis1 = -1 * round(self.gamepad.get_axis(0))#left x-axis + # input.axis2 = -1 * round(self.gamepad.get_axis(1))#left y-axis + # input.axis3 = -1 * round(self.gamepad.get_axis(4))#right y-axis #Button Mappings