execute extract and dispose tasks

README.md

@@ -19,3 +19,6 @@ Terminal 7:
 python3 aruco_navigator.py
 
 Terminal 8 (optional): Keyboard teleop
+
+cd /home/hello-robot/kevin/cse481/final_project && python3 ros_action_server.py
+ros2 topic pub --once /task_execution std_msgs/msg/String "data: 'disposal'"

final_project/aruco_data/receptacle_start.json

@@ -2,7 +2,7 @@
   "receptacle_start": {
     "frame": "receptacle",
     "position": {
-      "x": -0.5440935291934906,
+      "x": -0.6440935291934906,
       "y": -0.46971681621178407,
       "z": 0.6909102474383234
     },
@@ -29,7 +29,7 @@
       "joint_arm_l2": 0.07998252101556011,
       "joint_arm_l3": 0.07998252101556011,
       "joint_wrist_yaw": 1.5953400194010667,
-      "joint_wrist_pitch": 0.056757289151768725,
+      "joint_wrist_pitch": -1.4,
       "joint_wrist_roll": 0.0,
       "joint_gripper_finger_left": 0.06362513567124516,
       "joint_arm_total": 0.31993008406224044

final_project/aruco_data/receptacle_start_copy.json

@@ -0,0 +1,38 @@
+{
+  "receptacle_start": {
+    "frame": "receptacle",
+    "position": {
+      "x": -0.3,
+      "y": -0.46971681621178407,
+      "z": 0.4
+    },
+    "orientation": {
+      "x": -0.5014202917835091,
+      "y": 0.5083933888603739,
+      "z": 0.4909687286096096,
+      "w": 0.49906268211344834
+    },
+    "gripper_rpy": {
+      "joint_wrist_roll": 0.0,
+      "joint_wrist_pitch": -0.23316507975861747,
+      "joint_wrist_yaw": 3.5537221586017353
+    },
+    "lift_height": 1.0962894392744904,
+    "wrist_extension": 0.32049873355226427
+  },
+  "receptacle_drop": {
+    "tf": null,
+    "joints": {
+      "joint_lift": 1.0677495211483488,
+      "joint_arm_l0": 0.07998252101556011,
+      "joint_arm_l1": 0.07998252101556011,
+      "joint_arm_l2": 0.07998252101556011,
+      "joint_arm_l3": 0.07998252101556011,
+      "joint_wrist_yaw": 1.5953400194010667,
+      "joint_wrist_pitch": -0.4,
+      "joint_wrist_roll": 0.0,
+      "joint_gripper_finger_left": 0.06362513567124516,
+      "joint_arm_total": 0.31993008406224044
+    }
+  }
+}

final_project/aruco_data/trash_start.json

@@ -17,7 +17,7 @@
       "joint_wrist_pitch": -0.03681553890925539,
       "joint_wrist_yaw": 0.042184471666855135
     },
-    "lift_height": 0.6021245242705562,
+    "lift_height": 0.5021245242705562,
     "wrist_extension": 0.10000726175112523
   }
 }

final_project/frontend/final_frontend.css

@@ -0,0 +1,88 @@
+body { 
+    font-family: 'Segoe UI', Tahoma, Geneva, Verdana, sans-serif; 
+    background-color: #f4f4f9; 
+    color: #333; 
+    margin: 0; 
+    padding: 20px; 
+    display: flex; 
+    flex-direction: column; 
+    align-items: center; 
+}
+.container { 
+    background: white; 
+    padding: 20px; 
+    border-radius: 8px; 
+    box-shadow: 0 4px 6px rgba(0,0,0,0.1); 
+    width: 100%; 
+    max-width: 600px; 
+}
+h2 { 
+    color: #2c3e50; 
+    border-bottom: 2px solid #3498db; 
+    padding-bottom: 10px; 
+    margin-top: 0; 
+}
+.controls { 
+    display: grid; 
+    grid-template-columns: 1fr 1fr; 
+    gap: 10px; 
+    margin-bottom: 20px; 
+}
+button { 
+    padding: 12px; 
+    font-size: 16px; 
+    border: none; 
+    border-radius: 4px; 
+    cursor: pointer; 
+    transition: background 0.3s, transform 0.1s; 
+    background-color: #3498db; 
+    color: white; 
+}
+button:hover { 
+    background-color: #2980b9; 
+}
+button:active {
+    transform: translateY(2px);
+}
+button.secondary { 
+    background-color: #95a5a6; 
+}
+button.secondary:hover { 
+    background-color: #7f8c8d; 
+}
+button.danger { 
+    background-color: #e74c3c; 
+    grid-column: span 2; 
+    font-weight: bold; 
+    font-size: 18px;
+}
+button.danger:hover { 
+    background-color: #c0392b; 
+}
+button.primary { 
+    background-color: #2ecc71; 
+    grid-column: span 2; 
+    font-weight: bold; 
+    font-size: 18px;
+}
+button.primary:hover { 
+    background-color: #27ae60; 
+}
+.camera-feed { 
+    width: 100%; 
+    aspect-ratio: 4/3; 
+    background: #333; 
+    display: flex; 
+    align-items: center; 
+    justify-content: center; 
+    margin-top: 20px; 
+    border-radius: 4px; 
+    color: #fff; 
+    font-style: italic;
+    overflow: hidden;
+}
+.camera-feed img {
+    width: 100%;
+    height: 100%;
+    object-fit: contain;
+}

final_project/frontend/final_frontend.html

@@ -0,0 +1,118 @@
+<!DOCTYPE html>
+<html>
+<head>
+  <title>Automatic Waste Disposal</title>
+  <script src="https://cdn.jsdelivr.net/npm/roslib/build/roslib.min.js"></script>
+  <link rel="stylesheet" href="final_frontend.css">
+</head>
+<body>
+  <div class="container">
+    <h2>Automatic Waste Disposal System</h2>
+
+    <div class="controls">
+      <button class="primary" onclick="executeTask('sequence')">Start Automatic Disposal</button>
+      <!-- <button id="stopBtn" class="danger" onclick="estop()">Pause / Emergency Stop</button> -->
+      <button onclick="executeTask('extraction')">Execute Extraction</button>
+      <button onclick="executeTask('disposal')">Execute Disposal</button>
+      <button class="secondary" onclick="executeTask('reset')">Reset Robot (Stow)</button>
+      <button class="secondary" onclick="executeTask('locate_can')">Locate Garbage Can</button> 
+      <button class="secondary" onclick="executeTask('collect')">[FUTURE] Trash Collection</button>
+    </div>
+
+    <div style="display: flex; justify-content: space-between; align-items: center; margin-top: 20px; border-bottom: 2px solid #3498db; padding-bottom: 5px;">
+      <h3 style="margin: 0;">Live Camera Feed</h3>
+      <div>
+        <span id="status" style="font-family: monospace; margin-left: 5px;">Connecting...</span>
+      </div>
+    </div>
+
+    <div class="camera-feed" id="video">
+      <!-- 
+        If web_video_server is running:
+        <img src="http://slinky.hcrlab.cs.washington.edu:8080/stream?topic=/camera/color/image_raw"> 
+      -->
+      No Camera Feed Detected
+    </div>
+    </div>
+
+    <script>
+    const ros = new ROSLIB.Ros({
+      url: "ws://rocky.hcrlab.cs.washington.edu:9090"
+      //url: "ws://slinky.hcrlab.cs.washington.edu:9090"
+    });
+
+    const taskTopic = new ROSLIB.Topic({
+      ros: ros,
+      name: "/task_execution",
+      messageType: "std_msgs/String"
+    });
+
+    const statusText = document.getElementById("status");
+    // const stopBtn = document.getElementById("stopBtn");
+
+    ros.on('connection', () => {
+      statusText.innerText = "Connected to Stretch";
+      statusText.style.color = "#27ae60";
+    });
+
+    ros.on('error', (error) => {
+      statusText.innerText = "Error Connecting to Stretch";
+      statusText.style.color = "#e74c3c";
+    });
+
+    ros.on('close', () => {
+      statusText.innerText = "Connection Closed";
+      statusText.style.color = "#f39c12";
+    });
+
+    function executeTask(taskType) {
+      // Reset stop button appearance
+      /*
+      if (typeof stopBtn !== 'undefined' && stopBtn) {
+        stopBtn.innerText = "Pause / Emergency Stop";
+        stopBtn.style.backgroundColor = "#e74c3c";
+      }
+      */
+
+      const msg = new ROSLIB.Message({
+        data: taskType
+      });
+      taskTopic.publish(msg);
+      statusText.innerText = "Status: Executing " + taskType + "...";
+      console.log("Published task:", taskType);
+    }
+
+    /*
+    function estop() {
+      // Change button appearance to "Paused"
+      if (typeof stopBtn !== 'undefined' && stopBtn) {
+        stopBtn.innerText = "PAUSED";
+        stopBtn.style.backgroundColor = "#d35400";
+      }
+
+      // Send stop command to task execution
+      const msg = new ROSLIB.Message({
+        data: 'stop'
+      });
+      taskTopic.publish(msg);
+
+      // Also send zero velocity to cmd_vel as an immediate safety measure
+      const cmdVel = new ROSLIB.Topic({
+        ros: ros,
+        name: "/stretch/cmd_vel",
+        messageType: "geometry_msgs/Twist"
+      });
+      const stopMsg = new ROSLIB.Message({
+        linear: { x: 0, y: 0, z: 0 },
+        angular: { x: 0, y: 0, z: 0 }
+      });
+      cmdVel.publish(stopMsg);
+
+      statusText.innerText = "Status: EMERGENCY STOP SENT";
+      statusText.style.color = "#e74c3c";
+    }
+    */
+    </script>
+
+</body>
+</html>

final_project/joint_state_data/trash_pickup.json

@@ -15,7 +15,7 @@
       }
     },
     "joints": {
-      "joint_lift": 0.4182378365420798,
+      "joint_lift": 0.4282378365420798,
       "joint_arm_l0": 0.06851279826682022,
       "joint_arm_l1": 0.06851279826682022,
       "joint_arm_l2": 0.06851279826682022,
@@ -43,7 +43,7 @@
       }
     },
     "joints": {
-      "joint_lift": 0.4182378365420798,
+      "joint_lift": 0.9182378365420798,
       "joint_arm_l0": 0.06851279826682022,
       "joint_arm_l1": 0.06851279826682022,
       "joint_arm_l2": 0.06851279826682022,
@@ -71,12 +71,12 @@
       }
     },
     "joints": {
-      "joint_lift": 0.4182378365420798,
+      "joint_lift": 0.9182378365420798,
       "joint_arm_l0": 0.06851279826682022,
       "joint_arm_l1": 0.06851279826682022,
       "joint_arm_l2": 0.06851279826682022,
       "joint_arm_l3": 0.06851279826682022,
-      "joint_wrist_yaw": 0.042184471666855135,
+      "joint_wrist_yaw": 3.5537221586017353,
       "joint_wrist_pitch": -0.039883500485026674,
       "joint_wrist_roll": -0.0015339807878856412,
       "joint_gripper_finger_left": 0.0,

final_project/poses.json

@@ -0,0 +1,47 @@
+{
+  "receptacle_extend": {
+    "tf": null,
+    "joints": {
+      "joint_lift": 1.0919817209272333,
+      "joint_arm_l0": 0.03105301965281601,
+      "joint_arm_l1": 0.03105301965281601,
+      "joint_arm_l2": 0.03105301965281601,
+      "joint_arm_l3": 0.03105301965281601,
+      "joint_wrist_yaw": 0.06391586616190172,
+      "joint_wrist_pitch": 0.13499030933393644,
+      "joint_wrist_roll": 0.07516505860639641,
+      "joint_gripper_finger_left": -0.16389184517392533,
+      "joint_arm_total": 0.12421207861126404
+    }
+  },
+  "receptacle_retract": {
+    "tf": null,
+    "joints": {
+      "joint_lift": 1.092185060212284,
+      "joint_arm_l0": 0.002018066030862674,
+      "joint_arm_l1": 0.002018066030862674,
+      "joint_arm_l2": 0.002018066030862674,
+      "joint_arm_l3": 0.002018066030862674,
+      "joint_wrist_yaw": 0.06391586616190172,
+      "joint_wrist_pitch": 0.13345632854605077,
+      "joint_wrist_roll": 0.07516505860639641,
+      "joint_gripper_finger_left": -0.16389184517392533,
+      "joint_arm_total": 0.008072264123450697
+    }
+  },
+  "receptacle_start": {
+    "tf": null,
+    "joints": {
+      "joint_lift": 1.0922017440876965,
+      "joint_arm_l0": 0.0020171682355455784,
+      "joint_arm_l1": 0.0020171682355455784,
+      "joint_arm_l2": 0.0020171682355455784,
+      "joint_arm_l3": 0.0020171682355455784,
+      "joint_wrist_yaw": 0.06391586616190172,
+      "joint_wrist_pitch": 0.13345632854605077,
+      "joint_wrist_roll": 0.07516505860639641,
+      "joint_gripper_finger_left": -0.16389184517392533,
+      "joint_arm_total": 0.008068672942182314
+    }
+  }
+}