Gui cameras (#647)

* Graft changes from previous branch * GUI cleaned up, fixed message errors * Styled Code * Actually saved this time lol * Panorama button/rospy service implemented * Format --------- Co-authored-by: neuenfeldttj <[email protected]> Co-authored-by: qhdwight <[email protected]>
umrover · Feb 20, 2024 · 7cd08ae · 7cd08ae
1 parent 42b556b
commit 7cd08ae
Show file tree

Hide file tree

Showing 9 changed files with 235 additions and 316 deletions.
diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py
@@ -7,6 +7,8 @@
 from channels.generic.websocket import JsonWebsocketConsumer
 import rospy
 import tf2_ros
+import cv2
+from cv_bridge import CvBridge
 from geometry_msgs.msg import Twist
 from mrover.msg import (
     PDLB,
@@ -17,12 +19,15 @@
     StateMachineStateUpdate,
     Throttle,
     CalibrationStatus,
-    Calibrated,
     MotorsStatus,
+    CameraCmd,
+    Calibrated,
     Velocity,
     Position,
     IK,
 )
+from mrover.srv import EnableAuton, ChangeCameras, CapturePanorama
+from sensor_msgs.msg import NavSatFix, Temperature, RelativeHumidity, Image
 from mrover.srv import EnableAuton
 from sensor_msgs.msg import NavSatFix, Temperature, RelativeHumidity
 from std_msgs.msg import String, Bool
@@ -88,18 +93,15 @@ def connect(self):
                 "/sa_humidity_data", RelativeHumidity, self.sa_humidity_data_callback
             )
 
-            # Services
-            self.laser_service = rospy.ServiceProxy("enable_mosfet_device", SetBool)
-            self.calibrate_service = rospy.ServiceProxy("arm_calibrate", Trigger)
-            self.enable_auton = rospy.ServiceProxy("enable_auton", EnableAuton)
-
             # Services
             self.laser_service = rospy.ServiceProxy("enable_arm_laser", SetBool)
             self.enable_auton = rospy.ServiceProxy("enable_auton", EnableAuton)
             self.change_heater_srv = rospy.ServiceProxy("science_change_heater_auto_shutoff_state", SetBool)
             self.calibrate_cache_srv = rospy.ServiceProxy("cache_calibrate", Trigger)
             self.cache_enable_limit = rospy.ServiceProxy("cache_enable_limit_switches", SetBool)
             self.calibrate_service = rospy.ServiceProxy("arm_calibrate", Trigger)
+            self.change_cameras_srv = rospy.ServiceProxy("change_cameras", ChangeCameras)
+            self.capture_panorama_srv: Any = rospy.ServiceProxy("capture_panorama", CapturePanorama)
 
             # ROS Parameters
             self.mappings = rospy.get_param("teleop/joystick_mappings")
@@ -119,6 +121,7 @@ def connect(self):
             self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
             self.flight_thread = threading.Thread(target=self.flight_attitude_listener)
             self.flight_thread.start()
+
         except rospy.ROSException as e:
             rospy.logerr(e)
 
@@ -160,6 +163,12 @@ def receive(self, text_data):
                 self.auton_bearing()
             elif message["type"] == "mast_gimbal":
                 self.mast_gimbal(message)
+            elif message["type"] == "max_streams":
+                self.send_res_streams()
+            elif message["type"] == "sendCameras":
+                self.change_cameras(msg=message)
+            elif message["type"] == "center_map":
+                self.send_center()
             elif message["type"] == "enable_limit_switch":
                 self.limit_switch(message)
             elif message["type"] == "center_map":
@@ -588,10 +597,45 @@ def mast_gimbal(self, msg):
         up_down_pwr = msg["throttles"][1] * pwr["up_down_pwr"]
         self.mast_gimbal_pub.publish(Throttle(["mast_gimbal_x", "mast_gimbal_y"], [rot_pwr, up_down_pwr]))
 
-    def send_center(self):
-        lat = rospy.get_param("gps_linearization/reference_point_latitude")
-        long = rospy.get_param("gps_linearization/reference_point_longitude")
-        self.send(text_data=json.dumps({"type": "center_map", "latitude": lat, "longitude": long}))
+    def change_cameras(self, msg):
+        try:
+            camera_cmd = CameraCmd(msg["device"], msg["resolution"])
+            rospy.logerr(camera_cmd)
+            result = self.change_cameras_srv(primary=msg["primary"], camera_cmd=camera_cmd)
+            rospy.logerr(result)
+        except rospy.ServiceException as e:
+            print(f"Service call failed: {e}")
+
+    def send_res_streams(self):
+        res = rospy.get_param("cameras/max_num_resolutions")
+        streams = rospy.get_param("cameras/max_streams")
+        self.send(text_data=json.dumps({"type": "max_resolution", "res": res}))
+        self.send(text_data=json.dumps({"type": "max_streams", "streams": streams}))
+
+    def capture_panorama(self) -> None:
+        try:
+            response = self.capture_panorama_srv()
+            image = response.panorama
+            self.image_callback(image)
+        except rospy.ServiceException as e:
+            print(f"Service call failed: {e}")
+
+    def image_callback(self, msg):
+        bridge = CvBridge()
+        try:
+            # Convert the image to OpenCV standard format
+            cv_image = bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough")
+        except Exception as e:
+            rospy.logerr("Could not convert image message to OpenCV image: " + str(e))
+            return
+
+        # Save the image to a file (you could change 'png' to 'jpg' or other formats)
+        image_filename = "panorama.png"
+        try:
+            cv2.imwrite(image_filename, cv_image)
+            rospy.loginfo("Saved image to {}".format(image_filename))
+        except Exception as e:
+            rospy.logerr("Could not save image: " + str(e))
 
     def send_center(self):
         lat = rospy.get_param("gps_linearization/reference_point_latitude")

diff --git a/src/teleoperation/frontend/src/components/AutonTask.vue b/src/teleoperation/frontend/src/components/AutonTask.vue
@@ -212,7 +212,6 @@ export default defineComponent({
 }
 
 @keyframes blinkAnimation {
-
   0%,
   100% {
     background-color: var(--bs-success);
@@ -282,8 +281,8 @@ h2 {
   cursor: pointer;
 }
 
-.help:hover~.helpscreen,
-.help:hover~.helpimages {
+.help:hover ~ .helpscreen,
+.help:hover ~ .helpimages {
   visibility: visible;
 }
 

diff --git a/src/teleoperation/frontend/src/components/AutonWaypointEditor.vue b/src/teleoperation/frontend/src/components/AutonWaypointEditor.vue
@@ -9,25 +9,9 @@
           </div>
           <div class="form-group col-md-6">
             <label for="waypointid">Tag ID:</label>
-            <input
-              v-if="type == 1"
-              class="form-control"
-              id="waypointid"
-              v-model="id"
-              type="number"
-              max="249"
-              min="0"
-              step="1"
-            />
-            <input
-              v-else
-              class="form-control"
-              id="waypointid"
-              type="number"
-              placeholder="-1"
-              step="1"
-              disabled
-            />
+            <input v-if="type == 1" class="form-control" id="waypointid" v-model="id" type="number" max="249" min="0"
+              step="1" />
+            <input v-else class="form-control" id="waypointid" type="number" placeholder="-1" step="1" disabled />
           </div>
         </div>
 
@@ -39,33 +23,15 @@
         </select>
 
         <div class="form-check form-check-inline">
-          <input
-            v-model="odom_format_in"
-            class="form-check-input"
-            type="radio"
-            id="radioD"
-            value="D"
-          />
+          <input v-model="odom_format_in" class="form-check-input" type="radio" id="radioD" value="D" />
           <label class="form-check-label" for="radioD">D</label>
         </div>
         <div class="form-check form-check-inline">
-          <input
-            v-model="odom_format_in"
-            class="form-check-input"
-            type="radio"
-            id="radioDM"
-            value="DM"
-          />
+          <input v-model="odom_format_in" class="form-check-input" type="radio" id="radioDM" value="DM" />
           <label class="form-check-label" for="radioDM">DM</label>
         </div>
         <div class="form-check form-check-inline">
-          <input
-            v-model="odom_format_in"
-            class="form-check-input"
-            type="radio"
-            id="radioDMS"
-            value="DMS"
-          />
+          <input v-model="odom_format_in" class="form-check-input" type="radio" id="radioDMS" value="DMS" />
           <label class="form-check-label" for="radioDMS">DMS</label>
         </div>
 
@@ -119,55 +85,28 @@
             class="dragArea"
             draggable=".item'"
           > -->
-          <WaypointItem
-            v-for="(waypoint, i) in storedWaypoints"
-            :key="i"
-            :waypoint="waypoint"
-            :in_route="false"
-            :index="i"
-            @delete="deleteItem($event)"
-            @togglePost="togglePost($event)"
-            @add="addItem($event)"
-          />
+          <WaypointItem v-for="(waypoint, i) in storedWaypoints" :key="i" :waypoint="waypoint" :in_route="false"
+            :index="i" @delete="deleteItem($event)" @togglePost="togglePost($event)" @add="addItem($event)" />
           <!-- </draggable> -->
         </div>
       </div>
     </div>
     <div class="col-wrap" style="left: 50%">
       <!-- <div class="auton-check"> -->
-      <AutonModeCheckbox
-        ref="autonCheckbox"
-        class="auton-checkbox"
-        :name="autonButtonText"
-        :color="autonButtonColor"
-        @toggle="toggleAutonMode($event)"
-      />
+      <AutonModeCheckbox ref="autonCheckbox" class="auton-checkbox" :name="autonButtonText" :color="autonButtonColor"
+        @toggle="toggleAutonMode($event)" />
       <div class="stats">
         <VelocityCommand />
       </div>
-      <Checkbox
-        ref="teleopCheckbox"
-        class="teleop-checkbox"
-        :name="'Teleop Controls'"
-        @toggle="toggleTeleopMode($event)"
-      />
+      <Checkbox ref="teleopCheckbox" class="teleop-checkbox" :name="'Teleop Controls'"
+        @toggle="toggleTeleopMode($event)" />
       <!-- </div> -->
       <Checkbox class="stuck-checkbox" name="Stuck" @toggle="roverStuck = !roverStuck"></Checkbox>
       <h4 class="waypoint-headers">Current Course</h4>
       <div class="box route">
         <!-- <draggable v-model="route" class="dragArea" draggable=".item'"> -->
-        <WaypointItem
-          v-for="(waypoint, i) in route"
-          :id="id"
-          :key="i"
-          :waypoint="waypoint"
-          :in_route="true"
-          :index="i"
-          :name="name"
-          @delete="deleteItem($event)"
-          @togglePost="togglePost($event)"
-          @add="addItem($event)"
-        />
+        <WaypointItem v-for="(waypoint, i) in route" :id="id" :key="i" :waypoint="waypoint" :in_route="true" :index="i"
+          :name="name" @delete="deleteItem($event)" @togglePost="togglePost($event)" @add="addItem($event)" />
         <!-- </draggable> -->
       </div>
     </div>
@@ -186,94 +125,36 @@
                 <div class="modal-odom-format">
                   <h5>Waypoint format:</h5>
                   <div class="form-check form-check-inline">
-                    <input
-                      v-model="odom_format_in"
-                      class="form-check-input"
-                      type="radio"
-                      id="radioD"
-                      value="D"
-                    />
+                    <input v-model="odom_format_in" class="form-check-input" type="radio" id="radioD" value="D" />
                     <label class="form-check-label" for="radioD">D</label>
                   </div>
                   <div class="form-check form-check-inline">
-                    <input
-                      v-model="odom_format_in"
-                      class="form-check-input"
-                      type="radio"
-                      id="radioDM"
-                      value="DM"
-                    />
+                    <input v-model="odom_format_in" class="form-check-input" type="radio" id="radioDM" value="DM" />
                     <label class="form-check-label" for="radioDM">DM</label>
                   </div>
                   <div class="form-check form-check-inline">
-                    <input
-                      v-model="odom_format_in"
-                      class="form-check-input"
-                      type="radio"
-                      id="radioDMS"
-                      value="DMS"
-                    />
+                    <input v-model="odom_format_in" class="form-check-input" type="radio" id="radioDMS" value="DMS" />
                     <label class="form-check-label" for="radioDMS">DMS</label>
                   </div>
                 </div>
-                <div
-                  v-for="(header, index) in compModalHeaders"
-                  :key="header"
-                  class="comp-modal-inputs"
-                >
+                <div v-for="(header, index) in compModalHeaders" :key="header" class="comp-modal-inputs">
                   <h5>{{ header }}</h5>
-                  <input
-                    id="lat_deg"
-                    v-model.number="compModalLatDeg[index]"
-                    type="number"
-                    min="-90"
-                    max="90"
-                  />
+                  <input id="lat_deg" v-model.number="compModalLatDeg[index]" type="number" min="-90" max="90" />
                   <label>º</label>
-                  <input
-                    v-if="min_enabled"
-                    id="lat_min"
-                    v-model.number="compModalLatMin[index]"
-                    type="number"
-                    min="0"
-                    max="60"
-                  />
+                  <input v-if="min_enabled" id="lat_min" v-model.number="compModalLatMin[index]" type="number" min="0"
+                    max="60" />
                   <label v-if="min_enabled">'</label>
-                  <input
-                    v-if="sec_enabled"
-                    id="lat_sec"
-                    v-model.number="compModalLatSec[index]"
-                    type="number"
-                    min="0"
-                    max="3600"
-                  />
+                  <input v-if="sec_enabled" id="lat_sec" v-model.number="compModalLatSec[index]" type="number" min="0"
+                    max="3600" />
                   <label v-if="sec_enabled">"</label>
                   <label>N &nbsp; &nbsp;</label>
-                  <input
-                    id="lon_deg"
-                    v-model.number="compModalLonDeg[index]"
-                    type="number"
-                    min="-180"
-                    max="180"
-                  />
+                  <input id="lon_deg" v-model.number="compModalLonDeg[index]" type="number" min="-180" max="180" />
                   <label>º</label>
-                  <input
-                    v-if="min_enabled"
-                    id="lon_min"
-                    v-model.number="compModalLonMin[index]"
-                    type="number"
-                    min="0"
-                    max="60"
-                  />
+                  <input v-if="min_enabled" id="lon_min" v-model.number="compModalLonMin[index]" type="number" min="0"
+                    max="60" />
                   <label v-if="min_enabled">'</label>
-                  <input
-                    v-if="sec_enabled"
-                    id="lon_sec"
-                    v-model.number="compModalLonSec[index]"
-                    type="number"
-                    min="0"
-                    max="3600"
-                  />
+                  <input v-if="sec_enabled" id="lon_sec" v-model.number="compModalLonSec[index]" type="number" min="0"
+                    max="3600" />
                   <label v-if="sec_enabled">"</label>
                   <label>E</label>
                 </div>