Addressing Akshara's changes

Author: KavitShah1998

Diff for: spot_rl_experiments/spot_rl/baselines/go_to_waypoint.py

@@ -11,12 +11,12 @@
 def main(spot):
     parser = get_default_parser()
     parser.add_argument("-g", "--goal")
-    parser.add_argument("-w", "--waypoints")
+    parser.add_argument("-w", "--waypoint")
     parser.add_argument("-d", "--dock", action="store_true")
     parser.add_argument("-l", "--limit", action="store_true")
     args = parser.parse_args()
-    if args.waypoints is not None:
-        goal_x, goal_y, goal_heading = nav_target_from_waypoint(args.waypoints)
+    if args.waypoint is not None:
+        goal_x, goal_y, goal_heading = nav_target_from_waypoint(args.waypoint)
     else:
         assert args.goal is not None
         goal_x, goal_y, goal_heading = [float(i) for i in args.goal.split(",")]

Diff for: spot_rl_experiments/spot_rl/envs/nav_env.py

@@ -16,7 +16,7 @@
 
 DOCK_ID = int(os.environ.get("SPOT_DOCK_ID", 520))
 
-# Clean up this mess of paths
+# @Todo: Clean up this mess of paths
 this_dir = os.path.dirname(os.path.abspath(__file__))
 spot_rl_dir = os.path.join(os.path.dirname(this_dir))
 spot_rl_experiments_dir = os.path.join(os.path.dirname(spot_rl_dir))
@@ -43,7 +43,7 @@ def __init__(self, config, spot: Spot, record_trajectory=False) -> None:
         # Record robot's trajectory (i.e. waypoints)
         self.recording_in_progress = False
         self.start_time = 0.0
-        self.robot_trajectory = []  # type: list[dict]
+        self.robot_trajectory = []  # type: List[Dict]
         self.record_robot_trajectory = record_trajectory
 
         # Setup
@@ -89,9 +89,9 @@ def execute(self, nav_targets) -> List[Dict[str, str]]:
         # Return waypoints back
         return self.robot_trajectory
 
-    def shutdown(self, dock=False) -> None:
+    def shutdown(self, should_dock=False) -> None:
         try:
-            if dock:
+            if should_dock:
                 self.nav_env.say("Executing automatic docking")
                 dock_start_time = time.time()
                 while time.time() - dock_start_time < 2:
@@ -166,7 +166,7 @@ def get_observations(self):
         try:
             robot_trajectory = wp_controller.execute(nav_targets=nav_targets)
         finally:
-            wp_controller.shutdown(dock=args.dock)
+            wp_controller.shutdown(should_dock=args.dock)
 
         # @Todo: Make file_path as a compulsive argument when saving trajectory
         # @Todo: Add checks to ensure that filepath exists

Diff for: spot_rl_experiments/spot_rl/utils/calculate_distance.py

@@ -44,52 +44,22 @@ def calculate_dtw_distance_between_trajectories(test_trajectory, reference_traje
 
 def is_within_bounds(test_trajectory, reference_trajectories, threshold):
     """
-    Check if a test trajectory is within bounds of any reference trajectory.
+    Check if a test trajectory is not within bounds of any reference trajectory.
 
     Parameters:
     - test_trajectory (list): Test trajectory containing poses.
     - reference_trajectories (list): List of reference trajectories containing poses.
     - threshold (float): Threshold value for distance comparison.
 
     Returns:
-    - is_within (bool): True if the test trajectory is within bounds, False otherwise.
+    - is_within (bool): True if the test trajectory is within bounds for all reference trajectories, False otherwise.
     """
-    test_start_pose = (
-        test_trajectory[0]["pose"]["x"],
-        test_trajectory[0]["pose"]["y"],
-        test_trajectory[0]["pose"]["yaw"],
-    )
-    test_end_pose = (
-        test_trajectory[-1]["pose"]["x"],
-        test_trajectory[-1]["pose"]["y"],
-        test_trajectory[-1]["pose"]["yaw"],
-    )
-
     for reference_trajectory in reference_trajectories:
-        ref_start_pose = (
-            reference_trajectory[0]["pose"]["x"],
-            reference_trajectory[0]["pose"]["y"],
-            reference_trajectory[0]["pose"]["yaw"],
-        )
-        ref_end_pose = (
-            reference_trajectory[-1]["pose"]["x"],
-            reference_trajectory[-1]["pose"]["y"],
-            reference_trajectory[-1]["pose"]["yaw"],
+        dtw_dist = calculate_dtw_distance_between_trajectories(
+            test_trajectory, reference_trajectory
         )
-
-        start_distance = calculate_euclidean_distance_between_pose(
-            test_start_pose, ref_start_pose
-        )
-        end_distance = calculate_euclidean_distance_between_pose(
-            test_end_pose, ref_end_pose
-        )
-
-        if start_distance <= threshold and end_distance <= threshold:
-            dtw_dist = calculate_dtw_distance_between_trajectories(
-                test_trajectory, reference_trajectory
-            )
-            print(f"FastDTW dist: {dtw_dist}")
-            if dtw_dist > threshold:
-                return False
+        print(f"FastDTW dist: {dtw_dist}")
+        if dtw_dist > threshold:
+            return False
 
     return True

Diff for: spot_rl_experiments/spot_rl/utils/json_helpers.py

@@ -41,7 +41,7 @@ def load_json_file(file_path):
 
 def save_json_file(file_path, data):
     """
-    Save the contents of a Python object JSON file into a.
+    Save the contents of a Python object into a JSON file.
 
     Parameters:
     - file_path (str): Path to the JSON file.

Diff for: spot_rl_experiments/spot_rl/utils/utils.py

@@ -50,8 +50,10 @@ def construct_config(opts=None):
 def nav_target_from_waypoint(waypoint, waypoints=WAYPOINTS):
     nav_target = waypoints.get(waypoint)
     if nav_target is None:
+        default_waypoint = "dock"
+        nav_target = waypoints.get(default_waypoint)
         raise Exception(
-            f"waypoint - {waypoint} does not exist inside file waypoints.yaml, will go to `dock` waypoint instead"
+            f"waypoint - {waypoint} does not exist inside file waypoints.yaml, will go to {default_waypoint} waypoint instead"
         )
     goal_x, goal_y, goal_heading = nav_target
     return goal_x, goal_y, np.deg2rad(goal_heading)

Diff for: tests/hardware_tests/test_nav_env.py

@@ -52,7 +52,7 @@ def test_nav():
 
             # ENSURE ROBOT GOES BACK TO DOCK IN THIS CASE
         finally:
-            wp_controller.shutdown(dock=True)
+            wp_controller.shutdown(should_dock=True)
 
         print("test_robot_trajectory", robot_trajectory)
         assert robot_trajectory is not []