diff --git a/.gitmodules b/.gitmodules index 95602de..f45f5f3 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,3 @@ [submodule "src/astra_description"] path = src/astra_descriptions url = ../astra_descriptions -[submodule "src/astra_msgs"] - path = src/astra_msgs - url = git@github.com:SHC-ASTRA/astra_msgs.git diff --git a/flake.lock b/flake.lock index 401e99d..a940702 100644 --- a/flake.lock +++ b/flake.lock @@ -1,5 +1,29 @@ { "nodes": { + "astra-msgs": { + "inputs": { + "nix-ros-overlay": "nix-ros-overlay", + "nixpkgs": [ + "astra-msgs", + "nix-ros-overlay", + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1770494960, + "narHash": "sha256-0Q8uK1NtTSAh+Yoo7dnQk8aqfQb6uo0EGCWZdIIUrtQ=", + "owner": "SHC-ASTRA", + "repo": "astra_msgs", + "rev": "35d5ac0c2355cd9dc88ac746dbc627108a48bf81", + "type": "github" + }, + "original": { + "owner": "SHC-ASTRA", + "ref": "35d5ac0c2355cd9dc88ac746dbc627108a48bf81", + "repo": "astra_msgs", + "type": "github" + } + }, "flake-utils": { "inputs": { "systems": "systems" @@ -18,11 +42,49 @@ "type": "github" } }, + "flake-utils_2": { + "inputs": { + "systems": "systems_2" + }, + "locked": { + "lastModified": 1731533236, + "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=", + "owner": "numtide", + "repo": "flake-utils", + "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b", + "type": "github" + }, + "original": { + "owner": "numtide", + "repo": "flake-utils", + "type": "github" + } + }, "nix-ros-overlay": { "inputs": { "flake-utils": "flake-utils", "nixpkgs": "nixpkgs" }, + "locked": { + "lastModified": 1770408708, + "narHash": "sha256-E7ZQRgGrsiuswgXnG7337ZR5s4SdZlheZjxKOQdVoH8=", + "owner": "lopsided98", + "repo": "nix-ros-overlay", + "rev": "e78ba91032c7f8bdd823fbf43937cbf0f4f09747", + "type": "github" + }, + "original": { + "owner": "lopsided98", + "ref": "develop", + "repo": "nix-ros-overlay", + "type": "github" + } + }, + "nix-ros-overlay_2": { + "inputs": { + "flake-utils": "flake-utils_2", + "nixpkgs": "nixpkgs_2" + }, "locked": { "lastModified": 1761810010, "narHash": "sha256-o0wJKW603SiOO373BTgeZaF6nDxegMA/cRrzSC2Cscg=", @@ -39,6 +101,22 @@ } }, "nixpkgs": { + "locked": { + "lastModified": 1759381078, + "narHash": "sha256-gTrEEp5gEspIcCOx9PD8kMaF1iEmfBcTbO0Jag2QhQs=", + "owner": "NixOS", + "repo": "nixpkgs", + "rev": "7df7ff7d8e00218376575f0acdcc5d66741351ee", + "type": "github" + }, + "original": { + "owner": "lopsided98", + "ref": "nix-ros", + "repo": "nixpkgs", + "type": "github" + } + }, + "nixpkgs_2": { "locked": { "lastModified": 1744849697, "narHash": "sha256-S9hqvanPSeRu6R4cw0OhvH1rJ+4/s9xIban9C4ocM/0=", @@ -56,7 +134,8 @@ }, "root": { "inputs": { - "nix-ros-overlay": "nix-ros-overlay", + "astra-msgs": "astra-msgs", + "nix-ros-overlay": "nix-ros-overlay_2", "nixpkgs": [ "nix-ros-overlay", "nixpkgs" @@ -77,6 +156,21 @@ "repo": "default", "type": "github" } + }, + "systems_2": { + "locked": { + "lastModified": 1681028828, + "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", + "owner": "nix-systems", + "repo": "default", + "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", + "type": "github" + }, + "original": { + "owner": "nix-systems", + "repo": "default", + "type": "github" + } } }, "root": "root", diff --git a/flake.nix b/flake.nix index b6df787..9c4395b 100644 --- a/flake.nix +++ b/flake.nix @@ -4,6 +4,8 @@ inputs = { nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/master"; 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=35d5ac0c2355cd9dc88ac746dbc627108a48bf81"; }; outputs = @@ -11,6 +13,7 @@ self, nix-ros-overlay, nixpkgs, + astra-msgs, }: nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem ( system: diff --git a/src/astra_msgs b/src/astra_msgs deleted file mode 160000 index 91883d8..0000000 --- a/src/astra_msgs +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 91883d8cab275897be2ece5fc16d77f1baca34ed diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index d24546a..a4b9f4d 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -4,14 +4,13 @@ import threading import time import rclpy +from astra_msgs.action import BioVacuum +from astra_msgs.msg import BioControl, BioDistributor, VicCAN +from astra_msgs.srv import BioTestTube from rclpy.action import ActionServer from rclpy.node import Node from std_msgs.msg import Header, String -from astra_msgs.action import BioVacuum -from astra_msgs.msg import BioControl, BioDistributor, BioFeedback, VicCAN -from astra_msgs.srv import BioTestTube - serial_pub = None thread = None @@ -30,34 +29,27 @@ class SerialRelay(Node): # Initialize node super().__init__("bio_node") - # Get launch mode parameter - self.declare_parameter("launch_mode") - self.launch_mode = self.get_parameter("launch_mode").value - self.get_logger().info(f"bio launch_mode is: {self.launch_mode}") - # Anchor Topics - if self.launch_mode == "anchor": - self.anchor_fromvic_sub_ = self.create_subscription( - VicCAN, "/anchor/from_vic/bio", self.relay_fromvic, 20 - ) - self.anchor_tovic_pub_ = self.create_publisher( - VicCAN, "/anchor/to_vic/relay", 20 - ) + self.anchor_fromvic_sub_ = self.create_subscription( + VicCAN, "/anchor/from_vic/bio", self.relay_fromvic, 20 + ) + self.anchor_tovic_pub_ = self.create_publisher( + VicCAN, "/anchor/to_vic/relay", 20 + ) - self.anchor_sub = self.create_subscription( - String, "/anchor/bio/feedback", self.anchor_feedback, 10 - ) - self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10) + self.anchor_sub = self.create_subscription( + String, "/anchor/bio/feedback", self.anchor_feedback, 10 + ) + self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10) - self.distributor_sub = self.create_subscription( - BioDistributor, - "/bio/control/distributor", - self.distributor_callback, - 10, - ) + self.distributor_sub = self.create_subscription( + BioDistributor, + "/bio/control/distributor", + self.distributor_callback, + 10, + ) # Services - self.test_tube_service = self.create_service( BioTestTube, "/bio/control/test_tube", self.test_tube_callback ) @@ -82,12 +74,10 @@ class SerialRelay(Node): print("todo") def send_cmd(self, msg: str): - if ( - self.launch_mode == "anchor" - ): # if in anchor mode, send to anchor node to relay - output = String() - output.data = msg - self.anchor_pub.publish(output) + # send to anchor node to relay + output = String() + output.data = msg + self.anchor_pub.publish(output) def relay_fromvic(self, msg: VicCAN): # self.get_logger().info(msg) @@ -95,9 +85,6 @@ class SerialRelay(Node): self.process_fromvic_citadel(msg) def process_fromvic_citadel(self, msg: VicCAN): - if msg.mcu_name != "citadel": - return - # Check message len to prevent crashing on bad data if msg.command_id in viccan_citadel_msg_len_dict: expected_len = viccan_citadel_msg_len_dict[msg.command_id] @@ -105,7 +92,7 @@ class SerialRelay(Node): 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)})" ) - return + return def anchor_feedback(self, msg: String): output = msg.data @@ -120,9 +107,9 @@ class SerialRelay(Node): mcu_name="citadel", command_id=40, data=[ - to_short(distributor_arr[0]), - to_short(distributor_arr[1]), - to_short(distributor_arr[2]), + clamp_short(distributor_arr[0]), + clamp_short(distributor_arr[1]), + clamp_short(distributor_arr[2]), 0, ], ) @@ -205,7 +192,7 @@ class SerialRelay(Node): return BioVacuum.Result() -def to_short(x: int) -> int: +def clamp_short(x: int) -> int: return max(-32768, min(32767, x))