From 0ce872d9c9da1fe96beed3aef792379a19e4b097 Mon Sep 17 00:00:00 2001 From: iggy Date: Fri, 20 Feb 2026 04:54:37 -0600 Subject: [PATCH] added lance control, changed citadel to use uint8 instead of byte, removed astra_msgs submodule and added it as a nix package --- flake.lock | 20 +++++------ flake.nix | 56 +++++++++++++++--------------- src/bio_pkg/bio_pkg/bio_node.py | 60 ++++++++++++++++++++++++++++++--- 3 files changed, 95 insertions(+), 41 deletions(-) diff --git a/flake.lock b/flake.lock index f519579..ea7c48d 100644 --- a/flake.lock +++ b/flake.lock @@ -10,17 +10,17 @@ ] }, "locked": { - "lastModified": 1770693861, - "narHash": "sha256-8NBtSsKVYtd+NYt51tJ1EteC0nOTemDUBoRXbDoQAuA=", + "lastModified": 1771573274, + "narHash": "sha256-YlUVTWtCxXzobbcHTCrMMzwLSrzQwo5Yz57gYS8ie04=", "owner": "SHC-ASTRA", "repo": "astra_msgs", - "rev": "60bbb53085b09fbdb7e848b1dd168d526d9af281", + "rev": "8acbd807907ca6bd045aab8e50e7af995fc9a211", "type": "github" }, "original": { "owner": "SHC-ASTRA", - "ref": "60bbb53085b09fbdb7e848b1dd168d526d9af281", "repo": "astra_msgs", + "rev": "8acbd807907ca6bd045aab8e50e7af995fc9a211", "type": "github" } }, @@ -66,11 +66,11 @@ "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1770408708, - "narHash": "sha256-E7ZQRgGrsiuswgXnG7337ZR5s4SdZlheZjxKOQdVoH8=", + "lastModified": 1770622967, + "narHash": "sha256-1LYjTugPSCa/5NkP6/dcZLH5TQYj3R8mAZ/9dgd7jDM=", "owner": "lopsided98", "repo": "nix-ros-overlay", - "rev": "e78ba91032c7f8bdd823fbf43937cbf0f4f09747", + "rev": "d1b9f17eba909116356436d46b5192d299c6b49a", "type": "github" }, "original": { @@ -86,11 +86,11 @@ "nixpkgs": "nixpkgs_2" }, "locked": { - "lastModified": 1770108954, - "narHash": "sha256-VBj6bd4LPPSfsZJPa/UPPA92dOs6tmQo0XZKqfz/3W4=", + "lastModified": 1771404745, + "narHash": "sha256-UVP3TsQJ4PezyQG3B1SsgsTxz32XBVzplJ/cgq7v/uk=", "owner": "lopsided98", "repo": "nix-ros-overlay", - "rev": "3d05d46451b376e128a1553e78b8870c75d7753a", + "rev": "7cdf7b44ff186869baedbb3b82e5b409bb3e8dd9", "type": "github" }, "original": { diff --git a/flake.nix b/flake.nix index c5761f4..abbfea2 100644 --- a/flake.nix +++ b/flake.nix @@ -3,18 +3,13 @@ inputs = { nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/develop"; - nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!! - # specify astra_msgs commit hash to the one we support - astra-msgs.url = "github:SHC-ASTRA/astra_msgs?ref=60bbb53085b09fbdb7e848b1dd168d526d9af281"; + nixpkgs.follows = "nix-ros-overlay/nixpkgs"; + astra-msgs.url = + "github:SHC-ASTRA/astra_msgs/8acbd807907ca6bd045aab8e50e7af995fc9a211"; }; outputs = - { - self, - nix-ros-overlay, - nixpkgs, - astra-msgs, - }: + { self, nix-ros-overlay, nixpkgs, astra-msgs }: nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem ( system: let @@ -22,24 +17,27 @@ inherit system; overlays = [ nix-ros-overlay.overlays.default ]; }; + + astra_msgs_pkgs = astra-msgs.packages.${system}; + rosDistro = "humble"; in { devShells.default = pkgs.mkShell { name = "ASTRA Anchor"; + packages = with pkgs; [ - astra-msgs.packages.${system}.astra-msgs colcon - (python313.withPackages ( - p: with p; [ - pyserial - pygame - scipy - crccheck - black - ] - )) - ( - with rosPackages.humble; + astra_msgs_pkgs.astra-msgs + + (pkgs.rosPackages.${rosDistro}.python3.withPackages (p: with p; [ + pyserial + pygame + scipy + crccheck + black + ])) + + (with rosPackages.${rosDistro}; buildEnv { paths = [ ros-core @@ -66,7 +64,7 @@ moveit-msgs moveit-ros-planning moveit-ros-planning-interface - moveit-ros-visualization + moveit-ros-visualization moveit-configs-utils moveit-ros-move-group moveit-servo @@ -77,13 +75,16 @@ ompl joy ros2-controllers - chomp-motion-planner + chomp-motion-planner ]; - } - ) + }) ]; + + env = { + ASTRAMSGS = "${astra-msgs.outPath}"; + }; + shellHook = '' - # Display stuff export DISPLAY=''${DISPLAY:-:0} export QT_X11_NO_MITSHM=1 ''; @@ -93,6 +94,7 @@ nixConfig = { extra-substituters = [ "https://ros.cachix.org" ]; - extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ]; + extra-trusted-public-keys = + [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ]; }; } diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index a4b9f4d..1cc0cfc 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -5,7 +5,7 @@ import time import rclpy from astra_msgs.action import BioVacuum -from astra_msgs.msg import BioControl, BioDistributor, VicCAN +from astra_msgs.msg import BioControl, BioDistributor, FaerieControl, ScytheControl, VicCAN from astra_msgs.srv import BioTestTube from rclpy.action import ActionServer from rclpy.node import Node @@ -49,6 +49,20 @@ class SerialRelay(Node): 10, ) + self.anchor_sub = self.create_subscription( + FaerieControl, + "/bio/control/faerie", + self.faerie_callback, + 10 + ) + + self.anchor_sub = self.create_subscription( + ScytheControl, + "/bio/control/scythe", + self.scythe_callback, + 10 + ) + # Services self.test_tube_service = self.create_service( BioTestTube, "/bio/control/test_tube", self.test_tube_callback @@ -102,6 +116,7 @@ class SerialRelay(Node): def distributor_callback(self, msg: BioDistributor): distributor_arr = msg.distributor_id + # if (any in distributor > vic_cmd = VicCAN( header=Header(stamp=self.get_clock().now().to_msg()), mcu_name="citadel", @@ -114,6 +129,43 @@ class SerialRelay(Node): ], ) self.anchor_tovic_pub_.publish(vic_cmd) + + def faerie_callback(self, msg: FaerieControl): + if msg.vibration_motor: + vic_cmd = VicCAN( + header=Header(stamp=self.get_clock().now().to_msg()), + mcu_name="faerie", + command_id=37, + data=[float(msg.vibration_motor)] + ) + self.anchor_tovic_pub_.publish(vic_cmd) + else: + vic_cmd = VicCAN( + header=Header(stamp=self.get_clock().now().to_msg()), + mcu_name="faerie", + command_id=42, + data=[float(msg.move_faerie)], + ) + self.anchor_tovic_pub_.publish(vic_cmd) + + def scythe_callback(self, msg: ScytheControl): + if msg.scythe_reset: + vic_cmd = VicCAN( + header=Header(stamp=self.get_clock().now().to_msg()), + mcu_name="faerie", + command_id=29, + data=[], + ) + self.anchor_tovic_pub_.publish(vic_cmd) + else: + vic_cmd = VicCAN( + header=Header(stamp=self.get_clock().now().to_msg()), + mcu_name="faerie", + command_id=24, + data=[float(msg.move_scythe)], + ) + self.anchor_tovic_pub_.publish(vic_cmd) + def test_tube_callback(self, request, response): tubes_cmd = VicCAN( @@ -121,7 +173,7 @@ class SerialRelay(Node): mcu_name="citadel", command_id=40, data=[ - float(int.from_bytes(request.tube_id)), + float(int(request.tube_id)), float(request.milliliters), ], ) @@ -129,8 +181,8 @@ class SerialRelay(Node): return response def execute_vacuum(self, goal_handle): - valve_id = int.from_bytes(goal_handle.request.valve_id) - duty = int.from_bytes(goal_handle.request.fan_duty_cycle_percent) + valve_id = int(goal_handle.request.valve_id) + duty = int(goal_handle.request.fan_duty_cycle_percent) total = goal_handle.request.fan_time_ms # open valve