Skip to content

Commit

Permalink
navigator_drone_comm: stylistic changes
Browse files Browse the repository at this point in the history
  • Loading branch information
cbrxyz committed Dec 31, 2024
1 parent 169f8fe commit 292d817
Show file tree
Hide file tree
Showing 2 changed files with 18 additions and 18 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -28,12 +28,12 @@
TinPacket,
)

SendPackets = Union[HeartbeatSetPacket, EStopPacket, StartPacket, StopPacket]
RecvPackets = Union[HeartbeatReceivePacket, GPSDronePacket, TargetPacket, TinPacket]


class DroneCommDevice(
ROSSerialDevice[
Union[HeartbeatSetPacket, EStopPacket, StartPacket, StopPacket],
Union[HeartbeatReceivePacket, GPSDronePacket, TargetPacket, TinPacket],
],
ROSSerialDevice[SendPackets, RecvPackets],
):
def __init__(self, port: str):
super().__init__(port, 57600)
Expand All @@ -43,6 +43,7 @@ def __init__(self, port: str):
DroneTarget,
queue_size=5,
) # TODO replace with service call
self.target_msg = {}
self.tin_pub = rospy.Publisher("~tin", DroneTin, queue_size=5)
self.drone_heartbeat_pub = rospy.Publisher(
"~drone_heartbeat",
Expand Down Expand Up @@ -102,7 +103,7 @@ def stop_send(self):

def on_packet_received(
self,
packet: Union[HeartbeatReceivePacket, GPSDronePacket, TargetPacket, TinPacket],
packet: RecvPackets,
):
if isinstance(packet, HeartbeatReceivePacket):
rospy.loginfo("status %s", packet.status)
Expand All @@ -115,17 +116,21 @@ def on_packet_received(
point_msg = Point(packet.lat, packet.lon, packet.alt)
self.gps_pub.publish(point_msg)
elif isinstance(packet, TargetPacket):
rospy.loginfo(packet)
rospy.loginfo(str(packet))
self.target_msg[self.num_logos] = [
packet.lat,
packet.lon,
packet.logo.value,
]
self.num_logos += 1
rospy.loginfo(self.target_msg)
rospy.loginfo(str(self.target_msg))
# self.target_pub.publish(target_msg)
if self.num_logos > 1:
rospy.wait_for_service("uav_search_report_message")
drone_target_proxy = rospy.ServiceProxy(
"uav_search_report_message",
MessageUAVSearchReport,
)
drone_target_proxy.wait_for_service()
try:
drone_msg = MessageUAVSearchReportRequest()
drone_msg.object1 = self.target_msg[0][2]
Expand All @@ -141,12 +146,7 @@ def on_packet_received(
point2.altitude = 0.0
drone_msg.object1_location = point1
drone_msg.object2_location = point2

rospy.loginfo(drone_msg)
drone_target_proxy = rospy.ServiceProxy(
"uav_search_report_message",
MessageUAVSearchReport,
)
rospy.loginfo(str(drone_msg))
response = drone_target_proxy(drone_msg)
rospy.loginfo(response)
except Exception:
Expand All @@ -159,7 +159,7 @@ def on_packet_received(
uav_replenish_msg.item_status = packet.status
uav_replenish_msg.item_color = packet.color.name[0]

rospy.loginfo(uav_replenish_msg)
rospy.loginfo(str(uav_replenish_msg))
drone_tin_proxy = rospy.ServiceProxy(
"uav_replenishment_message",
MessageUAVReplenishment,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -82,11 +82,11 @@ class Color(Enum):

class Logo(Enum):
"""
R or N pad enum
Whether the drone detected the "R" logo pad, or the "N" logo pad.
"""

R_Logo = "R"
N_Logo = "N"
R_LOGO = "R"
N_LOGO = "N"


@dataclass
Expand Down

0 comments on commit 292d817

Please sign in to comment.