diff --git a/README.md b/README.md index 36793e6..9592b77 100644 --- a/README.md +++ b/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'" diff --git a/final_project/aruco_data/receptacle_start.json b/final_project/aruco_data/receptacle_start.json index a2f23fa..a4ff3c0 100644 --- a/final_project/aruco_data/receptacle_start.json +++ b/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 diff --git a/final_project/aruco_data/receptacle_start_copy.json b/final_project/aruco_data/receptacle_start_copy.json new file mode 100644 index 0000000..da8161d --- /dev/null +++ b/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 + } + } +} \ No newline at end of file diff --git a/final_project/aruco_data/trash_start.json b/final_project/aruco_data/trash_start.json index 4429490..d0d8519 100644 --- a/final_project/aruco_data/trash_start.json +++ b/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 } } \ No newline at end of file diff --git a/final_project/frontend/final_frontend.css b/final_project/frontend/final_frontend.css new file mode 100644 index 0000000..4064e88 --- /dev/null +++ b/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; +} \ No newline at end of file diff --git a/final_project/frontend/final_frontend.html b/final_project/frontend/final_frontend.html new file mode 100644 index 0000000..1a32c87 --- /dev/null +++ b/final_project/frontend/final_frontend.html @@ -0,0 +1,118 @@ + + + + Automatic Waste Disposal + + + + +
+

Automatic Waste Disposal System

+ +
+ + + + + + + +
+ +
+

Live Camera Feed

+
+ Connecting... +
+
+ +
+ + No Camera Feed Detected +
+
+ + + + + \ No newline at end of file diff --git a/final_project/joint_state_data/receptacle_drop.json b/final_project/joint_state_data/receptacle_drop.json deleted file mode 100644 index 3f3105a..0000000 --- a/final_project/joint_state_data/receptacle_drop.json +++ /dev/null @@ -1,17 +0,0 @@ -{ - "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.056757289151768725, - "joint_wrist_roll": 0.0, - "joint_gripper_finger_left": 0.06362513567124516, - "joint_arm_total": 0.31993008406224044 - } - } -} \ No newline at end of file diff --git a/final_project/joint_state_data/trash_pickup.json b/final_project/joint_state_data/trash_pickup.json index 530c025..1748d21 100644 --- a/final_project/joint_state_data/trash_pickup.json +++ b/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, diff --git a/final_project/poses.json b/final_project/poses.json new file mode 100644 index 0000000..245a8cf --- /dev/null +++ b/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 + } + } +} \ No newline at end of file diff --git a/final_project/ros_action_server.py b/final_project/ros_action_server.py index 484f245..68f2abd 100644 --- a/final_project/ros_action_server.py +++ b/final_project/ros_action_server.py @@ -1,55 +1,312 @@ +import json import rclpy from rclpy.node import Node -from rclpy.action import ActionServer, GoalResponse +from std_msgs.msg import String +from sensor_msgs.msg import JointState + +import tf2_ros +from tf2_ros import TransformException, Buffer, TransformListener +from tf_transformations import euler_from_quaternion, quaternion_matrix + +import sys +import time +import threading # <-- Added to handle blocking action calls safely +from math import atan2, sqrt +import numpy as np +from control_msgs.action import FollowJointTrajectory +from geometry_msgs.msg import TransformStamped +from rclpy.action import ActionClient +from rclpy.duration import Duration +from rclpy.time import Time +from trajectory_msgs.msg import JointTrajectoryPoint +from action_msgs.msg import GoalStatus + +CAN_START_POSE_FILE = "/home/hello-robot/kevin/cse481/final_project/aruco_data/trash_start.json" # this is how stretch approaches the can +CAN_PICKUP_POSE_FILE = "/home/hello-robot/kevin/cse481/final_project/joint_state_data/trash_pickup.json" # this is the extraction poses +RECEPTACLE_START_POSE_FILE = "/home/hello-robot/kevin/cse481/final_project/aruco_data/receptacle_start.json" # this is the approach pose for the receptacle + +TRASH_CAN_OFFSET_ORIENTATION = np.pi + np.pi/4 # added np.pi/4 because friction and turning issues +RECEPTACLE_OFFSET_ORIENTATION = np.pi/2 class WasteDisposal(Node): def __init__(self): super().__init__('waste_disposal') - # Action Server - self._action_server = ActionServer( + # TF and Action Client setup + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + self.trajectory_client = ActionClient( self, - None, # Placeholder Action Type + FollowJointTrajectory, + "/stretch_controller/follow_joint_trajectory", + ) + + if not self.trajectory_client.wait_for_server(timeout_sec=10.0): + self.get_logger().error("Unable to connect to trajectory server.") + + # Subscriber + self.subscription = self.create_subscription( + String, 'task_execution', - execute_callback=self.execute_callback, - goal_callback=self.goal_callback + self.task_callback, + 10 ) - self.get_logger().info("Action server started.") - - def goal_callback(self, goal_request): - return GoalResponse.ACCEPT - - def execute_callback(self, goal_handle): - """route incoming goals to correct handler""" - task_type = goal_handle.request.task_type - # navigation, extraction, etc. + self.get_logger().info("Waste Disposal node started and listening to /task_execution.") + + def task_callback(self, msg): + task_type = msg.data.strip().lower() + self.get_logger().info(f"Received task: {task_type}") + handler = getattr(self, f"execute_{task_type}", None) if not handler: self.get_logger().error(f"Unknown task type: {task_type}") - goal_handle.abort() - return None + return - handler(goal_handle) - return None # should return result - - def execute_navigation(self, goal_handle): - self.get_logger().info("Executing navigation task ...") + # Run execution in a separate background thread so rclpy.spin() doesn't deadlock + threading.Thread(target=handler, daemon=True).start() + + def load_poses(self, file_path): + try: + with open(file_path, "r") as f: + return json.load(f) + except Exception as e: + self.get_logger().error(f"Could not load poses from {file_path}: {e}") + return {} + + def send_base_goal_blocking(self, joints_list): + point = JointTrajectoryPoint() + point.positions = [float(inc) for _, inc in joints_list] + point.time_from_start = Duration(seconds=5.0).to_msg() + + goal = FollowJointTrajectory.Goal() + goal.trajectory.joint_names = [joint_name for joint_name, _ in joints_list] + goal.trajectory.points = [point] + + joint_names_str = ", ".join(goal.trajectory.joint_names) + self.get_logger().info(f"Sending goal for joints: [{joint_names_str}]") + + send_goal_future = self.trajectory_client.send_goal_async(goal) + + # Use a passive sleep loop instead of spin_until_future_complete to avoid deadlock + while not send_goal_future.done(): + time.sleep(0.1) + + goal_handle = send_goal_future.result() + + if not goal_handle.accepted: + self.get_logger().error(f"Goal for joints [{joint_names_str}] was rejected!") + return False + + result_future = goal_handle.get_result_async() + while not result_future.done(): + time.sleep(0.1) + + result = result_future.result() + + if result.status != GoalStatus.STATUS_SUCCEEDED: + self.get_logger().warn(f"Goal for joints [{joint_names_str}] did not succeed: status {result.status}") + return False + + self.get_logger().info(f"Goal for joints [{joint_names_str}] succeeded.") + return True + + def compute_difference(self, target_frame, offset_x=0, offset_y=0, offset_z=0, offset_orientation=0): + try: + self.get_logger().info(f"aligning to offsets offset_x {offset_x}, offset_y {offset_y}, offset_z {offset_z}") + + # Extract quaternion and rotation matrix of marker in base_link frame + trans_base = self.tf_buffer.lookup_transform( + "base_link", target_frame, Time() + ) + x, y, z, w = ( + trans_base.transform.rotation.x, + trans_base.transform.rotation.y, + trans_base.transform.rotation.z, + trans_base.transform.rotation.w, + ) + R = quaternion_matrix((x, y, z, w)) + + # Apply rotation to the offset vector, positive Z DIRECTION IN OUR CASE + P_dash = np.array([[offset_x], [offset_y], [offset_z], [1]]) + P = np.array( + [ + [trans_base.transform.translation.x], + [trans_base.transform.translation.y], + [0], + [1], + ] + ) + X = np.matmul(R, P_dash) + + # Compute the marker position with offset in base_link frame + P_base = X + P + P_base[3, 0] = 1 # Homogeneous coordinate + + # Extract adjusted position + base_position_x = P_base[0, 0] + base_position_y = P_base[1, 0] + + # Compute rotation and translation needed + phi = atan2(base_position_y, base_position_x) + dist = sqrt(base_position_x**2 + base_position_y**2) + + _, _, z_rot_base = euler_from_quaternion([x, y, z, w]) + # Calculate final rotation: -phi (cancel rotation needed to align), + # + z_rot_base (original marker rotation), + # + pi (such that the base and the marker axis are aligned as shown in tutorial) + + # offset_orientation: np.pi for trash can start, np.pi/2 for receptacle + z_rot_base = -phi + z_rot_base + offset_orientation + + return phi, dist, z_rot_base + except TransformException as e: + self.get_logger().error(f"Transform error: {e}") + return None, None, None + + def align_to_marker(self, target_frame, offset_x=0, offset_y=0, offset_z=0, offset_orientation=0): + self.get_logger().info(f"Aligning to {target_frame} with offset z={offset_z}") + phi, dist, final_theta = self.compute_difference(target_frame, offset_x, offset_y, offset_z, offset_orientation) + + if phi is None: + return False + + # Split base goals because they are mutually exclusive in the hardware controller + self.send_base_goal_blocking([("rotate_mobile_base", phi)]) + self.send_base_goal_blocking([("translate_mobile_base", dist)]) + self.send_base_goal_blocking([("rotate_mobile_base", final_theta)]) + return True + + def execute_named_pose_from_dict(self, pose_data): + if "joints" in pose_data: + joints = pose_data["joints"] + + def get_joint(key, default): + if key not in joints: + self.get_logger().warn(f"Joint '{key}' not found in joints dict, using default: {default}") + return joints.get(key, default) + + lift_val = get_joint("joint_lift", 0.0) + arm_total = get_joint("joint_arm_total", 0.0) + yaw_val = get_joint("joint_wrist_yaw", 0.0) + pitch_val = get_joint("joint_wrist_pitch", 0.0) + roll_val = get_joint("joint_wrist_roll", 0.0) + else: + gripper_rpy = pose_data.get("gripper_rpy", {}) + + def get_flat(key, default): + if key not in pose_data: + self.get_logger().warn(f"Key '{key}' not found in pose_data, using default: {default}") + return pose_data.get(key, default) + + def get_rpy(key, default): + if key not in gripper_rpy: + self.get_logger().warn(f"Key '{key}' not found in gripper_rpy, using default: {default}") + return gripper_rpy.get(key, default) + + lift_val = get_flat("lift_height", 0.0) + arm_total = get_flat("wrist_extension", 0.0) + yaw_val = get_rpy("joint_wrist_yaw", 0.0) + pitch_val = get_rpy("joint_wrist_pitch", 0.0) + roll_val = get_rpy("joint_wrist_roll", 0.0) + + arm_segment = arm_total / 4.0 + joints_list = [ + ("joint_lift", lift_val), + ("joint_arm_l0", arm_segment), + ("joint_arm_l1", arm_segment), + ("joint_arm_l2", arm_segment), + ("joint_arm_l3", arm_segment), + ("joint_wrist_yaw", yaw_val), + ("joint_wrist_pitch", pitch_val), + ("joint_wrist_roll", roll_val), + ] + + self.get_logger().info(f"Joints list: {joints_list}") + return self.send_base_goal_blocking(joints_list) + + def execute_extraction(self): + # Approach + self.get_logger().info("Executing navigation (approaching trash can)...") + start_poses = self.load_poses(CAN_START_POSE_FILE) + + if "trash_start" in start_poses: + pose = start_poses["trash_start"] + target_frame = pose.get("frame", "trash_can") + offset_z = pose.get("position", {}).get("z") + if self.align_to_marker(target_frame, offset_z=offset_z, offset_orientation=TRASH_CAN_OFFSET_ORIENTATION): + self.execute_named_pose_from_dict(pose) + + self.send_base_goal_blocking([("translate_mobile_base", -0.1)]) + + # Extraction + self.get_logger().info("Executing extraction (picking up trash)...") + pickup_poses = self.load_poses(CAN_PICKUP_POSE_FILE) + # Sequence: before_pickup -> (grip) -> pickup_high -> pickup_retracted + for pose_name in ["before_pickup", "pickup_high", "pickup_retracted"]: + if pose_name in pickup_poses: + self.get_logger().info(f"Executing pose: {pose_name}") + self.execute_named_pose_from_dict(pickup_poses[pose_name]) + time.sleep(5.0) + + def execute_disposal(self): + # Approach + self.get_logger().info("Executing navigation (approaching receptacle)...") + poses = self.load_poses(RECEPTACLE_START_POSE_FILE) + + if "receptacle_start" in poses: + start_pose = poses["receptacle_start"] + target_frame = start_pose.get("frame", "receptacle") + + offset_z = start_pose.get("position", {}).get("z", 0.0) + offset_x = start_pose.get("position", {}).get("x", 0.0) + if self.align_to_marker(target_frame, offset_x=offset_x, offset_z=offset_z, offset_orientation=RECEPTACLE_OFFSET_ORIENTATION): + self.execute_named_pose_from_dict(start_pose) + self.send_base_goal_blocking([("translate_mobile_base", 0.9)]) # move forward + time.sleep(2.0) + # disposal is in same JSON as approach + self.get_logger().info("Executing disposal (dropping into receptacle)...") - goal_handle.succeed() - return None + if "receptacle_drop" in poses: + drop_pose = poses["receptacle_drop"] + self.execute_named_pose_from_dict(drop_pose) - def execute_extraction(self, goal_handle): - self.get_logger().info("Executing extraction task...") + def execute_sequence(self): + self.get_logger().info("Starting automatic sequence: Extraction -> Disposal") + self.execute_extraction() + self.execute_disposal() + self.get_logger().info("Automatic sequence completed.") + def execute_reset(self): + self.get_logger().info("Executing reset (returning to neutral pose)...") + joints_list = [ + ("joint_lift", 0.5), + ("joint_arm_l0", 0.0), + ("joint_arm_l1", 0.0), + ("joint_arm_l2", 0.0), + ("joint_arm_l3", 0.0), + ("joint_wrist_yaw", 0.0), + ("joint_wrist_pitch", 0.0), + ("joint_wrist_roll", 0.0), + ] + self.send_base_goal_blocking(joints_list) - goal_handle.succeed() - return None + # def execute_stop(self): + # self.get_logger().warn("Stop requested! Halting immediately.") + # if self.trajectory_client: + # self.get_logger().info("Attempting to cancel current trajectory...") + def main(args=None): rclpy.init(args=args) - rclpy.spin(WasteDisposal()) + waste_disposal = WasteDisposal() + try: + rclpy.spin(waste_disposal) + except KeyboardInterrupt: + pass + waste_disposal.destroy_node() rclpy.shutdown() if __name__ == '__main__': diff --git a/final_project/simple_navigation.py b/final_project/simple_navigation.py new file mode 100644 index 0000000..fc3e07a --- /dev/null +++ b/final_project/simple_navigation.py @@ -0,0 +1,55 @@ +#! /usr/bin/env python3 +from copy import deepcopy +from geometry_msgs.msg import PoseStamped +from stretch_nav2.robot_navigator import BasicNavigator, TaskResult +import rclpy +from rclpy.duration import Duration + +def main(): + rclpy.init() + navigator = BasicNavigator() + + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 0.0 + initial_pose.pose.position.y = 0.0 + initial_pose.pose.orientation.z = 0.0 + initial_pose.pose.orientation.w = 1.0 + navigator.setInitialPose(initial_pose) + + navigator.waitUntilNav2Active() + + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'map' + goal_pose.header.stamp = navigator.get_clock().now().to_msg() + goal_pose.pose.position.x = 1.8 + goal_pose.pose.position.y = 1.8 + goal_pose.pose.orientation.w = 1.0 + + navigator.get_logger().info('Going to receptacle...') + navigator.goToPose(goal_pose) + + i = 0 + nav_start = navigator.get_clock().now() + while not navigator.isTaskComplete(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + navigator.get_logger().info('Navigating to goal...') + if navigator.get_clock().now() - nav_start > Duration(seconds=60.0): + navigator.get_logger().warn('Timed out, cancelling.') + navigator.cancelTask() + + result = navigator.getResult() + if result == TaskResult.SUCCEEDED: + navigator.get_logger().info('Reached receptacle!') + elif result == TaskResult.CANCELED: + navigator.get_logger().info('Navigation canceled.') + elif result == TaskResult.FAILED: + navigator.get_logger().info('Navigation failed.') + + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/final_project/startup.sh b/final_project/startup.sh index a7b67ad..3a89107 100755 --- a/final_project/startup.sh +++ b/final_project/startup.sh @@ -2,17 +2,20 @@ # Check argument if [ "$1" == "slinky" ]; then - WEB_DIR="$HOME/bresenham/lab13" + WEB_DIR="$HOME/kevin/cse481/final_project" TARGET_URL="http://slinky.hcrlab.cs.washington.edu:8000" elif [ "$1" == "weird-stretch" ]; then - WEB_DIR="$HOME/bresenham/cse481/lab13" + WEB_DIR="$HOME/bresenham/cse481/final_project" TARGET_URL="http://weird-stretch.cs.washington.edu:8000" +elif [ "$1" == "rocky" ]; then + WEB_DIR="$HOME/kevin/cse481/final_project" + TARGET_URL="http://rocky.hcrlab.cs.washington.edu:8000" else - echo "Usage: $0 [slinky|weird-stretch]" + echo "Usage: $0 [slinky|weird-stretch|rocky]" exit 1 fi -echo "If necessary, uncomment correct url in lab13/pose_manager_frontend.html" +# echo "If necessary, uncomment correct url in lab13/pose_manager_frontend.html" SESSION="dev" @@ -47,12 +50,9 @@ tmux send-keys -t "$SESSION:rosbridge" "ros2 launch rosbridge_server rosbridge_w tmux new-window -t "$SESSION" -n 'webserver' tmux send-keys -t "$SESSION:webserver" "cd ${WEB_DIR} && python3 -m http.server 8000" C-m -# Opens browser -xdg-open "$TARGET_URL" - -# Terminal 7: navigator -tmux new-window -t "$SESSION" -n 'navigator' -tmux send-keys -t "$SESSION:navigator" "cd ${WEB_DIR} && python3 aruco_navigator.py" C-m +# Terminal 7: action_server +tmux new-window -t "$SESSION" -n 'action_server' +tmux send-keys -t "$SESSION:action_server" "cd ${WEB_DIR} && python3 ros_action_server.py" C-m # Terminal 8: keyboard_teleop tmux new-window -t "$SESSION" -n 'keyboard_teleop' diff --git a/lab13/aruco_navigator.py b/lab13/aruco_navigator.py index 14c9153..54ef087 100644 --- a/lab13/aruco_navigator.py +++ b/lab13/aruco_navigator.py @@ -180,7 +180,7 @@ def compute_difference(self, offset_x = 0, offset_y = 0, offset_z = 0): R = quaternion_matrix((x, y, z, w)) # Apply rotation to the offset vector, positive Z DIRECTION IN OUR CASE - P_dash = np.array([[0], [0], [offset_z], [1]]) + P_dash = np.array([[offset_x], [offset_y], [offset_z], [1]]) P = np.array( [ [trans_base.transform.translation.x], diff --git a/lab13/pose_manager_frontend.html b/lab13/pose_manager_frontend.html index 419692d..ad3c91a 100644 --- a/lab13/pose_manager_frontend.html +++ b/lab13/pose_manager_frontend.html @@ -40,7 +40,8 @@

Sequence Builder