merged jnn/click-ik

scripts/debug_service.py

@@ -6,16 +6,16 @@
 
 from typing import Any
 import rospy
-from std_srvs.srv import SetBool, SetBoolResponse
+from mrover.srv import ChangeCameras, ChangeCamerasResponse
 
 # Change these values for the service name and type definition to test different values
-SERVICE_NAME = "science_enable_heater_b1"
-SERVICE_TYPE = SetBool
+SERVICE_NAME = "change_cameras"
+SERVICE_TYPE = ChangeCameras
 
 
 def print_service_request(service_request: Any):
     rospy.loginfo(service_request)
-    return SetBoolResponse(success=True)
+    return ChangeCamerasResponse(success=True)
 
 
 def main():

src/teleoperation/backend/consumers.py

@@ -28,12 +28,14 @@
     Velocity,
     Position,
     IK,
-    ClickIkAction,
-    ClickIkGoal,
     Spectral,
     ScienceThermistors,
     HeaterData,
+    ClickIkAction,
+    ClickIkGoal,
+    ClickIkFeedback,
 )
+import actionlib
 from mrover.srv import EnableAuton, AdjustMotor, ChangeCameras, CapturePanorama
 from sensor_msgs.msg import NavSatFix, Temperature, RelativeHumidity, Image
 from std_msgs.msg import String
@@ -97,9 +99,6 @@ def connect(self):
             self.sa_humidity_data = rospy.Subscriber(
                 "/sa_humidity_data", RelativeHumidity, self.sa_humidity_data_callback
             )
-
-            # Action clients
-            self.click_ik_client = actionlib.SimpleActionClient('do_click_ik', ClickIkAction)
             self.ish_thermistor_data = rospy.Subscriber(
                 "/science_thermistors", ScienceThermistors, self.ish_thermistor_data_callback
             )
@@ -109,6 +108,9 @@ def connect(self):
             self.science_spectral = rospy.Subscriber("/science_spectral", Spectral, self.science_spectral_callback)
             self.cmd_vel = rospy.Subscriber("/cmd_vel", Twist, self.cmd_vel_callback)
 
+            # Action clients
+            self.click_ik_client = actionlib.SimpleActionClient('do_click_ik', ClickIkAction)
+
             # Services
             self.laser_service = rospy.ServiceProxy("enable_arm_laser", SetBool)
             self.enable_auton = rospy.ServiceProxy("enable_auton", EnableAuton)
@@ -693,14 +695,6 @@ def capture_panorama(self) -> None:
         except rospy.ServiceException as e:
             print(f"Service call failed: {e}")
 
-    # def capture_photo(self):
-    #     try:
-    #         response = self.capture_photo_srv()
-    #         image = response.photo
-    #         self.image_callback(image)
-    #     except rospy.ServiceException as e:
-    #         print(f"Service call failed: {e}")
-
     # def image_callback(self, msg):
     #     bridge = CvBridge()
     #     try:
@@ -816,7 +810,9 @@ def start_click_ik(self, msg) -> None:
         goal = ClickIkGoal()
         goal.pointInImageX = msg["data"]["x"]
         goal.pointInImageY = msg["data"]["y"]
-        self.click_ik_client.send_goal(goal)
+        def feedback_cb(feedback: ClickIkFeedback) -> None:
+            self.send(text_data=json.dumps({"type": "click_ik_feedback", "distance": feedback.distance}))
+        self.click_ik_client.send_goal(goal, feedback_cb=feedback_cb)
 
     def cancel_click_ik(self, msg) -> None:
         self.click_ik_client.cancel_all_goals()

src/teleoperation/frontend/src/components/CameraFeed.vue

@@ -44,16 +44,17 @@ export default defineComponent({
   components: {
     Checkbox
   },
-  
+
   data() {
     return {
-      IKCam: false,
       ws: null as WebSocket | null,
       isUnmounted: false,
 
       // IK Mode
       IKCam: false,
 
+      // SA/ISH Mode
+      site: 'A',
       quality: 2
     }
   },
@@ -65,18 +66,42 @@ export default defineComponent({
 
   mounted: function () {
     this.startStream(this.id)
-    // this.$nextTick(() => {
-    //   const canvas: HTMLCanvasElement = document.getElementById(
-    //     'stream-' + this.id
-    //   ) as HTMLCanvasElement
-    //   const context = canvas.getContext('2d') ?? new CanvasRenderingContext2D()
-    //   context.fillStyle = 'black'
-    //   context.fillRect(0, 0, canvas.width, canvas.height)
-    // })
+    this.$nextTick(() => {
+      const canvas: HTMLCanvasElement = document.getElementById(
+        'stream-' + this.id
+      ) as HTMLCanvasElement
+      const context = canvas.getContext('2d') ?? new CanvasRenderingContext2D()
+      context.fillStyle = 'black'
+      context.fillRect(0, 0, canvas.width, canvas.height)
+    })
   },
+
   methods: {
     ...mapActions('websocket', ['sendMessage']),
 
+    downloadScreenshot: function () {
+      const currentdate = new Date()
+      const dateString =
+        currentdate.getMonth() +
+        1 +
+        '-' +
+        currentdate.getDate() +
+        '-' +
+        currentdate.getFullYear() +
+        ' @ ' +
+        currentdate.getHours() +
+        ':' +
+        currentdate.getMinutes() +
+        ':' +
+        currentdate.getSeconds()
+      var link = document.createElement('a')
+      console.log('dateString', dateString)
+      link.download = 'Site_' + this.site + ' ' + dateString + '.png'
+      let canvas = document.getElementById('stream-' + this.id) as HTMLCanvasElement
+      link.href = canvas.toDataURL()
+      link.click()
+      link.remove()
+    },
     handleClick: function (event: MouseEvent) {
       if (this.IKCam && this.mission === 'ik') {
         this.sendMessage({ type: 'start_click_ik', data: { x: event.offsetX, y: event.offsetY } })
@@ -90,11 +115,11 @@ export default defineComponent({
         resolution: parseInt(this.quality)
       })
     },
-    
+
     toggleIKMode: function () {
       this.IKCam = !this.IKCam
     },
-
+    
     startStream(number: Number) {
       // This function is called as a retry when the websocket closes
       // If our component goes away (unmounts) we should stop trying to reconnect
@@ -259,9 +284,5 @@ canvas {
   width:640px;
   height:480px;
 }
-canvas {
-  width: 640px;
-  height: 480px;
-}
 </style>
 

src/teleoperation/frontend/src/components/Cameras.vue

@@ -39,12 +39,6 @@
     </div>
     <div class="col">
       <h3>All Cameras</h3>
-      <div class="info">
-      <template v-for="i in camsEnabled.length" :key="i">
-        <CameraInfo v-if="camsEnabled[i - 1]" :id="i - 1" :name="names[i - 1]" :stream="getStreamNum(i - 1)"
-          @newQuality="changeQuality($event)" @swapStream="swapStream($event)"></CameraInfo>
-      </template>
-      </div>
       <div class="d-flex justify-content-end" v-if="isSA">
         <button class="btn btn-primary btn-lg custom-btn" @click="takePanorama()">
           Take Panorama