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

added support for laser, added drill_speed, removed scythe reset

Browse Source
This commit is contained in:
iggy
2026-02-22 06:49:32 -06:00
parent 0ce872d9c9
commit caab0ebd7a
1 changed files with 54 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

93
src/bio_pkg/bio_pkg/bio_node.py
View File

@@ -5,7 +5,7 @@ import time
import rclpy import rclpy
from astra_msgs.action import BioVacuum from astra_msgs.action import BioVacuum
from astra_msgs.msg import BioControl, BioDistributor, FaerieControl, ScytheControl, VicCAN from astra_msgs.msg import BioControl, BioDistributor, BioLaser, FaerieControl, ScytheControl, VicCAN
from astra_msgs.srv import BioTestTube from astra_msgs.srv import BioTestTube
from rclpy.action import ActionServer from rclpy.action import ActionServer
from rclpy.node import Node from rclpy.node import Node
@@ -49,18 +49,25 @@ class SerialRelay(Node):
10, 10,
) )
self.anchor_sub = self.create_subscription( self.faerie_sub = self.create_subscription(
FaerieControl, FaerieControl,
"/bio/control/faerie", "/bio/control/faerie",
self.faerie_callback, self.faerie_callback,
10 10,
) )
self.anchor_sub = self.create_subscription( self.scythe_sub = self.create_subscription(
ScytheControl, ScytheControl,
"/bio/control/scythe", "/bio/control/scythe",
self.scythe_callback, self.scythe_callback,
10 10,
)
self.laser_sub = self.create_subscription(
ScytheControl,
"/bio/control/laser",
self.laser_callback,
10,
) )
# Services # Services
@@ -131,44 +138,52 @@ class SerialRelay(Node):
self.anchor_tovic_pub_.publish(vic_cmd) self.anchor_tovic_pub_.publish(vic_cmd)
def faerie_callback(self, msg: FaerieControl): def faerie_callback(self, msg: FaerieControl):
if msg.vibration_motor: # msg.vibration_motor:
vic_cmd = VicCAN( vic_cmd = VicCAN(
header=Header(stamp=self.get_clock().now().to_msg()), header=Header(stamp=self.get_clock().now().to_msg()),
mcu_name="faerie", mcu_name="faerie",
command_id=37, command_id=37,
data=[float(msg.vibration_motor)] data=[float(msg.vibration_motor)]
) )
self.anchor_tovic_pub_.publish(vic_cmd) self.anchor_tovic_pub_.publish(vic_cmd)
else: # msg.move_faerie:
vic_cmd = VicCAN( vic_cmd = VicCAN(
header=Header(stamp=self.get_clock().now().to_msg()), header=Header(stamp=self.get_clock().now().to_msg()),
mcu_name="faerie", mcu_name="faerie",
command_id=42, command_id=42,
data=[float(msg.move_faerie)], data=[float(msg.move_faerie)],
) )
self.anchor_tovic_pub_.publish(vic_cmd) self.anchor_tovic_pub_.publish(vic_cmd)
# msg.drill_speed
vic_cmd = VicCAN(
header=Header(stamp=self.get_clock().now().to_msg()),
mcu_name="faerie",
command_id=19,
data=[float(msg.drill_speed)],
)
self.anchor_tovic_pub_.publish(vic_cmd)
def scythe_callback(self, msg: ScytheControl): def scythe_callback(self, msg: ScytheControl):
if msg.scythe_reset: vic_cmd = VicCAN(
vic_cmd = VicCAN( header=Header(stamp=self.get_clock().now().to_msg()),
header=Header(stamp=self.get_clock().now().to_msg()), mcu_name="faerie",
mcu_name="faerie", command_id=24,
command_id=29, data=[float(msg.move_scythe)],
data=[], )
) self.anchor_tovic_pub_.publish(vic_cmd)
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 laser_callback(self, msg: BioLaser):
vic_cmd = VicCAN(
header=Header(stamp=self.get_clock().now().to_msg()),
mcu_name="faerie",
command_id=24,
data=[float(msg.fire)],
)
self.anchor_tovic_pub_.publish(vic_cmd)
def test_tube_callback(self, request, response): def test_tube_callback(self, request, response):
tubes_cmd = VicCAN( vic_cmd = VicCAN(
header=Header(stamp=self.get_clock().now().to_msg()), header=Header(stamp=self.get_clock().now().to_msg()),
mcu_name="citadel", mcu_name="citadel",
command_id=40, command_id=40,
@@ -177,7 +192,7 @@ class SerialRelay(Node):
float(request.milliliters), float(request.milliliters),
], ],
) )
self.anchor_tovic_pub_.publish(tubes_cmd) self.anchor_tovic_pub_.publish(vic_cmd)
return response return response
def execute_vacuum(self, goal_handle): def execute_vacuum(self, goal_handle):