diff --git a/rover/system_statuses/msg/RoverSysStatus.msg b/rover/system_statuses/msg/RoverSysStatus.msg index bb8c85b..d181327 100644 --- a/rover/system_statuses/msg/RoverSysStatus.msg +++ b/rover/system_statuses/msg/RoverSysStatus.msg @@ -1,4 +1,19 @@ +int8 UTC_GPS_time +int8 bogie_connection_1 +int8 bogie_connection_2 +int8 bogie_connection_3 +int8 arm_connection_status +int8 arm_end_effector_connection_statuses int8 camera_zed int8 camera_undercarriage int8 camera_chassis int8 camera_main_navigation +int8 sample_containment_connection_status +int8 tower_connection_status +int8 chassis_pan_tilt_connection_status +int8 GPS_connection_status +int8 jetson_CPU +int8 jetson_RAM +int8 jetson_EMMC +int8 jetson_NVME_SSD +int8 FrSky_controller_connection_status \ No newline at end of file diff --git a/rover/system_statuses/scripts/.idea/workspace.xml b/rover/system_statuses/scripts/.idea/workspace.xml index 4a2352c..f4b4512 100644 --- a/rover/system_statuses/scripts/.idea/workspace.xml +++ b/rover/system_statuses/scripts/.idea/workspace.xml @@ -14,8 +14,8 @@ - - + + @@ -24,9 +24,11 @@ - - - + + + + + @@ -49,8 +51,8 @@ DEFINITION_ORDER - @@ -118,7 +120,7 @@ - + @@ -130,6 +132,7 @@ + @@ -139,7 +142,6 @@ - @@ -159,16 +161,18 @@ - - - + + + + + - - + + diff --git a/rover/system_statuses/scripts/system_statuses_node.py b/rover/system_statuses/scripts/system_statuses_node.py index c9ca9b5..23e8368 100755 --- a/rover/system_statuses/scripts/system_statuses_node.py +++ b/rover/system_statuses/scripts/system_statuses_node.py @@ -21,7 +21,9 @@ class SystemStatuses: rospy.Subscriber('camera_2_changer_chatter', Camera2Changer, self.__sub_callback) self.msg = RoverSysStatus() - self.__set_cameras() + self.__set_msg_values() + # self.__set_cameras() + # self.__UTC_GPS_Time() # Simple Subscriber test boolean -- camera_2_updated.py # self.hasCameraUndercarriageChanged = False @@ -34,18 +36,79 @@ class SystemStatuses: self.msg.camera_undercarriage = data.camera_2_value self.hasCameraUndercarriageChanged = True + # INIT all RoverSysMessage values + def __set_msg_values(self): + self.__utc_gps_time() + self.__bogie_connection_statuses() + self.__arm_connection_status() + self.__arm_end_effector_connection_statuses() + self.__set_cameras() + self.__sample_containment_connection_status() + self.__tower_connection_status() + self.__chassis_pan_tilt_connection_status() + self.__gps_connection_status() + self.__jetson_usage_information() + self.__frsky_controller_connection_status() + + # Pulls the UTC GPS Time (WIP) + def __utc_gps_time(self): + self.msg.UTC_GPS_time = 0 + + # Pulls bogie connection statuses (WIP) + def __bogie_connection_statuses(self): + self.msg.bogie_connection_1 = 0 + self.msg.bogie_connection_2 = 0 + self.msg.bogie_connection_3 = 0 + + # Checks arm connection status (WIP) + def __arm_connection_status(self): + self.msg.arm_connection_status = 0 + + # Checks Arm End Effector Connection Statuses (WIP) + def __arm_end_effector_connection_statuses(self): + self.msg.arm_end_effector_connection_statuses = 0 + + # Sets the Camera values (zed, undercarriage, chassis, and main_nav def __set_cameras(self): - # Check if camera_zed is found - self.msg.camera_zed = 1 if os.path.exists(self.system_path_locations[0]) else 0 - # Check if camera_undercarriage is found - self.msg.camera_undercarriage = 1 if os.path.exists(self.system_path_locations[1]) else 0 - # Check if camera_chassis is found - self.msg.camera_chassis = 1 if os.path.exists(self.system_path_locations[2]) else 0 - # Check if camera_main_navigation is found - self.msg.camera_main_navigation = 1 if os.path.exists(self.system_path_locations[3]) else 0 + # Check if camera_zed is found + self.msg.camera_zed = 1 if os.path.exists(self.system_path_locations[0]) else 0 + # Check if camera_undercarriage is found + self.msg.camera_undercarriage = 1 if os.path.exists(self.system_path_locations[1]) else 0 + # Check if camera_chassis is found + self.msg.camera_chassis = 1 if os.path.exists(self.system_path_locations[2]) else 0 + # Check if camera_main_navigation is found + self.msg.camera_main_navigation = 1 if os.path.exists(self.system_path_locations[3]) else 0 + + # Checks Sample Containment Connection Status (WIP) + def __sample_containment_connection_status(self): + self.msg.sample_containment_connection_status = 0 + + def __tower_connection_status(self): + self.msg.tower_connection_status = 0 + + # Checks Tower Connection Status (WIP) + def __chassis_pan_tilt_connection_status(self): + self.msg.chassis_pan_tilt_connection_status = 0 + + # Checks GPS Status (WIP) + def __gps_connection_status(self): + self.msg.GPS_connection_status = 0 + + # Get Jetson Statuses (WIP) + def __jetson_usage_information(self): + self.msg.jetson_CPU = 0 + self.msg.jetson_RAM = 0 + self.msg.jetson_EMMC = 0 + self.msg.jetson_NVME_SSD = 0 + + # Check FrSky Controller Connection Status (WIP) + def __frsky_controller_connection_status(self): + self.msg.FrSky_controller_connection_status = 0 def run(self): r = rospy.Rate(10) + rospy.loginfo(self.msg) + self.pub.publish(self.msg) while not rospy.is_shutdown(): if (self.msg.camera_zed != os.path.exists(self.system_path_locations[0]) or self.msg.camera_undercarriage != os.path.exists(self.system_path_locations[1]) or