From 52e17669d162c9aeca2a2891c9d03d7a36ef0f9c Mon Sep 17 00:00:00 2001
From: MatthewTaylor24 <matcurtay@comcast.net>
Date: Sat, 13 Jan 2018 19:23:09 -0800
Subject: [PATCH] First attempt: checks if camera is added/removed & updates

---
 .../scripts/.idea/workspace.xml               |  8 ++---
 .../scripts/system_statuses_node.py           | 33 ++++++++++---------
 2 files changed, 21 insertions(+), 20 deletions(-)

diff --git a/rover/system_statuses/scripts/.idea/workspace.xml b/rover/system_statuses/scripts/.idea/workspace.xml
index d52433c..4a2352c 100644
--- a/rover/system_statuses/scripts/.idea/workspace.xml
+++ b/rover/system_statuses/scripts/.idea/workspace.xml
@@ -14,8 +14,8 @@
       <file leaf-file-name="system_statuses_node.py" pinned="false" current-in-tab="true">
         <entry file="file://$PROJECT_DIR$/system_statuses_node.py">
           <provider selected="true" editor-type-id="text-editor">
-            <state relative-caret-position="504">
-              <caret line="31" column="0" lean-forward="true" selection-start-line="31" selection-start-column="0" selection-end-line="31" selection-end-column="0" />
+            <state relative-caret-position="84">
+              <caret line="29" column="48" lean-forward="false" selection-start-line="29" selection-start-column="48" selection-end-line="29" selection-end-column="48" />
               <folding />
             </state>
           </provider>
@@ -167,8 +167,8 @@
     </entry>
     <entry file="file://$PROJECT_DIR$/system_statuses_node.py">
       <provider selected="true" editor-type-id="text-editor">
-        <state relative-caret-position="504">
-          <caret line="31" column="0" lean-forward="true" selection-start-line="31" selection-start-column="0" selection-end-line="31" selection-end-column="0" />
+        <state relative-caret-position="84">
+          <caret line="29" column="48" lean-forward="false" selection-start-line="29" selection-start-column="48" selection-end-line="29" selection-end-column="48" />
           <folding />
         </state>
       </provider>
diff --git a/rover/system_statuses/scripts/system_statuses_node.py b/rover/system_statuses/scripts/system_statuses_node.py
index 01f7329..c9ca9b5 100755
--- a/rover/system_statuses/scripts/system_statuses_node.py
+++ b/rover/system_statuses/scripts/system_statuses_node.py
@@ -21,22 +21,10 @@ class SystemStatuses:
         rospy.Subscriber('camera_2_changer_chatter', Camera2Changer, self.__sub_callback)
 
         self.msg = RoverSysStatus()
-        # 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
-
-        # self.msg.camera_zed = 0
-        # self.msg.camera_undercarriage = 0
-        # self.msg.camera_gimbal = 0
-        # self.msg.camera_main_navigation = 0
+        self.__set_cameras()
 
         # Simple Subscriber test boolean -- camera_2_updated.py
-        self.hasCameraUndercarriageChanged = False
+        # self.hasCameraUndercarriageChanged = False
 
     # Callback for test Subscription of camera_2_updater.py -- checks if camera_undercarriage
     # has changed, updates value, and updates bool for run->pub.publish to execute
@@ -46,13 +34,26 @@ class SystemStatuses:
             self.msg.camera_undercarriage = data.camera_2_value
             self.hasCameraUndercarriageChanged = True
 
+    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
+
     def run(self):
         r = rospy.Rate(10)
         while not rospy.is_shutdown():
-            if self.hasCameraUndercarriageChanged:
+            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
+                    self.msg.camera_chassis != os.path.exists(self.system_path_locations[2]) or
+                    self.msg.camera_main_navigation != os.path.exists(self.system_path_locations[3])):
+                self.__set_cameras()
                 rospy.loginfo(self.msg)
                 self.pub.publish(self.msg)
-                self.hasCameraUndercarriageChanged = False
             r.sleep()