format code, change image sub name, fix spelling

src/navigation/approach_post.py

@@ -1,9 +1,9 @@
 import tf2_ros
-import rospy
-from context import Context
 from aenum import Enum, NoAlias
 from geometry_msgs.msg import Twist
 from util.ros_utils import get_rosparam
+
+from context import Context
 from state import BaseState
 
 

src/navigation/context.py

@@ -1,25 +1,23 @@
 from __future__ import annotations
-from pdb import post_mortem
-import rospy
-import tf2_ros
-from geometry_msgs.msg import Twist
+
+from dataclasses import dataclass
+from typing import ClassVar, Optional, List, Tuple
+
 import mrover.msg
 import mrover.srv
-from util.SE3 import SE3
-from visualization_msgs.msg import Marker
-from typing import ClassVar, Optional, List, Tuple
 import numpy as np
-from dataclasses import dataclass
-from shapely.geometry import Point, LineString
-from mrover.msg import Waypoint, GPSWaypoint, EnableAuton, WaypointType, GPSPointList
 import pymap3d
-from drive import DriveController
-from util.ros_utils import get_rosparam
-
+import rospy
+import tf2_ros
+from geometry_msgs.msg import Twist
+from mrover.msg import Waypoint, GPSWaypoint, EnableAuton, WaypointType, GPSPointList
+from shapely.geometry import Point
 from std_msgs.msg import Time, Bool
-from drive import DriveController
+from util.SE3 import SE3
 from util.ros_utils import get_rosparam
+from visualization_msgs.msg import Marker
 
+from drive import DriveController
 
 TAG_EXPIRATION_TIME_SECONDS = 60
 

src/navigation/drive.py

@@ -1,9 +1,9 @@
 from __future__ import annotations
-from typing import Tuple, Optional
-import numpy as np
-import rospy
+
 from enum import Enum
+from typing import Tuple, Optional
 
+import numpy as np
 from geometry_msgs.msg import Twist
 from util.SE3 import SE3
 from util.np_utils import angle_to_rotate, normalized

src/navigation/failure_identification/failure_identification.py

@@ -1,20 +1,21 @@
 #!/usr/bin/env python3
-import rospy
+
+from pathlib import Path
+from typing import Optional
+
 import message_filters
-from mrover.msg import MotorsStatus
+import numpy as np
+import pandas as pd
+import rospy
 from geometry_msgs.msg import Twist
+from mrover.msg import MotorsStatus
 from nav_msgs.msg import Odometry
+from pandas import DataFrame
 from smach_msgs.msg import SmachContainerStatus
 from std_msgs.msg import Bool
-import pandas as pd
-from pandas import DataFrame
-from watchdog import WatchDog
-import numpy as np
-import os
-from pathlib import Path
 from util.ros_utils import get_rosparam
-from util.SO3 import SO3
-from typing import Optional
+
+from watchdog import WatchDog
 
 DATAFRAME_MAX_SIZE = get_rosparam("failure_identification/dataframe_max_size", 200)
 POST_RECOVERY_GRACE_PERIOD = get_rosparam("failure_identification/post_recovery_grace_period", 5.0)

src/navigation/failure_identification/watchdog.py

@@ -1,9 +1,9 @@
+from typing import Tuple
+
 import numpy as np
-import rospy
 from pandas import DataFrame
-from util.ros_utils import get_rosparam
 from util.SO3 import SO3
-from typing import Tuple
+from util.ros_utils import get_rosparam
 
 WINDOW_SIZE = get_rosparam("watchdog/window_size", 100)
 ANGULAR_THRESHOLD = get_rosparam("watchdog/angular_threshold", 0.001)

src/navigation/gate.py

@@ -1,21 +1,18 @@
 from __future__ import annotations
-from typing import ClassVar, Optional
-from unicodedata import normalize
-from context import Gate, Context
 
-import numpy as np
-import rospy
+from dataclasses import dataclass
+from typing import Optional
 
-from context import Context, Environment, Rover, convert_cartesian_to_gps
+import numpy as np
 from aenum import Enum, NoAlias
-from state import BaseState
-from trajectory import Trajectory
-from dataclasses import dataclass
-from drive import DriveController
+from mrover.msg import GPSPointList
+from shapely.geometry import LineString, Polygon, Point
 from util.np_utils import normalized, perpendicular_2d
 from util.ros_utils import get_rosparam
-from shapely.geometry import LineString, Polygon, Point
-from mrover.msg import GPSPointList
+
+from context import Context, Rover, convert_cartesian_to_gps
+from context import Gate
+from state import BaseState
 
 STOP_THRESH = get_rosparam("gate/stop_thresh", 0.2)
 DRIVE_FWD_THRESH = get_rosparam("gate/drive_fwd_thresh", 0.34)  # 20 degrees

src/navigation/navigation.py

@@ -7,18 +7,19 @@
 import rospy
 import smach
 import smach_ros
+from navigation.state import DoneState, DoneStateTransitions, OffState, OffStateTransitions
+from smach.log import loginfo
+from smach.log import set_loggers
+from std_msgs.msg import String
+
+from approach_post import ApproachPostState, ApproachPostStateTransitions
 from context import Context
 from gate import GateTraverseState, GateTraverseStateTransitions
-from approach_post import ApproachPostState, ApproachPostStateTransitions
-from navigation.state import DoneState, DoneStateTransitions, OffState, OffStateTransitions
-from waypoint import WaypointState, WaypointStateTransitions
-from search import SearchState, SearchStateTransitions
-from recovery import RecoveryState, RecoveryStateTransitions
 from partial_gate import PartialGateState, PartialGateStateTransitions
 from post_backup import PostBackupState, PostBackupTransitions
-from smach.log import set_loggers
-from smach.log import loginfo, logwarn, logerr
-from std_msgs.msg import String
+from recovery import RecoveryState, RecoveryStateTransitions
+from search import SearchState, SearchStateTransitions
+from waypoint import WaypointState, WaypointStateTransitions
 
 
 class Navigation(threading.Thread):

src/navigation/partial_gate.py

@@ -1,12 +1,13 @@
 from dataclasses import dataclass
-import numpy as np
+from typing import Optional
 
+import numpy as np
+from aenum import Enum, NoAlias
 from util import np_utils
-from typing import Optional
+
+from context import Context
 from state import BaseState
 from trajectory import Trajectory
-from aenum import Enum, NoAlias
-from context import Context
 
 STOP_THRESH = 0.2
 DRIVE_FWD_THRESH = 0.95

src/navigation/post_backup.py

@@ -1,17 +1,20 @@
 from __future__ import annotations
+
 from dataclasses import dataclass
-import numpy as np
 from typing import Optional
-from state import BaseState
-from trajectory import Trajectory
+
+import numpy as np
+import rospy
+import tf2_ros
 from aenum import Enum, NoAlias
-from context import Context
-from util.ros_utils import get_rosparam
-from util.np_utils import perpendicular_2d
 from shapely.geometry import Point, LineString
 from util.SE3 import SE3
-import tf2_ros
-import rospy
+from util.np_utils import perpendicular_2d
+from util.ros_utils import get_rosparam
+
+from context import Context
+from state import BaseState
+from trajectory import Trajectory
 
 POST_RADIUS = get_rosparam("gate/post_radius", 0.7) * get_rosparam("single_fiducial/post_avoidance_multiplier", 1.42)
 BACKUP_DISTANCE = get_rosparam("recovery/recovery_distance", 2.0)

src/navigation/recovery.py

@@ -1,15 +1,14 @@
-import smach
-from typing import List
 from context import Context
-from aenum import Enum, NoAlias
-from state import BaseState
-from geometry_msgs.msg import Twist
-import rospy
-import numpy as np
 from typing import Optional
-from util.np_utils import perpendicular_2d, rotate_2d
+
+import numpy as np
+import rospy
+from aenum import Enum, NoAlias
+from util.np_utils import rotate_2d
 from util.ros_utils import get_rosparam
 
+from context import Context
+from state import BaseState
 
 STOP_THRESH = get_rosparam("recovery/stop_thresh", 0.2)
 DRIVE_FWD_THRESH = get_rosparam("recovery/drive_fwd_thresh", 0.34)  # 20 degrees

src/navigation/search.py

@@ -1,15 +1,16 @@
 from __future__ import annotations
+
+from dataclasses import dataclass
 from typing import Optional
 
 import numpy as np
+from aenum import Enum, NoAlias
+from mrover.msg import GPSPointList
+from util.ros_utils import get_rosparam
 
 from context import Context, convert_cartesian_to_gps
-from aenum import Enum, NoAlias
 from state import BaseState
-from dataclasses import dataclass
 from trajectory import Trajectory
-from mrover.msg import GPSPointList
-from util.ros_utils import get_rosparam
 
 
 @dataclass

src/navigation/state.py

@@ -1,11 +1,12 @@
-from abc import ABC
+from abc import ABC, abstractmethod
 from typing import List, Optional
 
 import smach
-from context import Context
 from aenum import Enum, NoAlias
 from geometry_msgs.msg import Twist
 
+from context import Context
+
 
 class BaseState(smach.State, ABC):
     """
@@ -70,9 +71,10 @@ def reset(self):
         """
         pass
 
+    @abstractmethod
     def evaluate(self, ud: smach.UserData) -> str:
         """Override me instead of execute!"""
-        raise NotImplementedError
+        ...
 
 
 class DoneStateTransitions(Enum):

src/navigation/trajectory.py

@@ -1,4 +1,5 @@
 from dataclasses import dataclass, field
+
 import numpy as np
 
 

src/navigation/waypoint.py

@@ -1,12 +1,12 @@
 from typing import List, Optional
 
-import tf2_ros
-
 import numpy as np
+import tf2_ros
 from aenum import Enum, NoAlias
+from util.ros_utils import get_rosparam
+
 from context import Context
 from state import BaseState
-from util.ros_utils import get_rosparam
 
 
 class WaypointStateTransitions(Enum):

starter_project/autonomy/config/starter_project.rviz

@@ -173,7 +173,7 @@ Visualization Manager:
       Value: false
     - Class: rviz/Image
       Enabled: true
-      Image Topic: /camera/color/image_raw
+      Image Topic: /camera/right/image
       Max Value: 1
       Median window: 5
       Min Value: 0

starter_project/autonomy/format.sh

@@ -0,0 +1,44 @@
+#!/usr/bin/env bash
+
+# See: https://vaneyckt.io/posts/safer_bash_scripts_with_set_euxo_pipefail/
+set -Eeuo pipefail
+
+readonly RED='\033[0;31m'
+readonly NC='\033[0m'
+
+BLACK_ARGS=(
+  "--line-length=120"
+  "--color"
+)
+
+# Just do a dry run if the "fix" argument is not passed
+if [ $# -eq 0 ] || [ "$1" != "--fix" ]; then
+  BLACK_ARGS+=("--diff") # Show difference
+  BLACK_ARGS+=("--check") # Exit with non-zero code if changes are required (for CI)
+fi
+
+function print_update_error() {
+  echo -e "${RED}[Error] Please update with ./ansible.sh build.yml${NC}"
+  exit 1
+}
+
+function find_executable() {
+  local readonly executable="$1"
+  local readonly version="$2"
+  local readonly path=$(which "${executable}")
+  if [ ! -x "${path}" ]; then
+    echo -e "${RED}[Error] Could not find ${executable}${NC}"
+    print_update_error
+  fi
+  if ! "${path}" --version | grep -q "${version}"; then
+    echo -e "${RED}[Error] Wrong ${executable} version${NC}"
+    print_update_error
+  fi
+  echo "${path}"
+}
+
+readonly BLACK_PATH=$(find_executable black 23.9.1)
+
+echo
+echo "Style checking Python with black ..."
+"${BLACK_PATH}" "${BLACK_ARGS[@]}" ./src

starter_project/autonomy/lint.sh

@@ -0,0 +1,33 @@
+#!/usr/bin/env bash
+
+# See: https://vaneyckt.io/posts/safer_bash_scripts_with_set_euxo_pipefail/
+set -Eeuo pipefail
+
+readonly RED='\033[0;31m'
+readonly NC='\033[0m'
+
+function print_update_error() {
+  echo -e "${RED}[Error] Please update with ./ansible.sh build.yml${NC}"
+  exit 1
+}
+
+function find_executable() {
+  local readonly executable="$1"
+  local readonly version="$2"
+  local readonly path=$(which "${executable}")
+  if [ ! -x "${path}" ]; then
+    echo -e "${RED}[Error] Could not find ${executable}${NC}"
+    print_update_error
+  fi
+  if ! "${path}" --version | grep -q "${version}"; then
+    echo -e "${RED}[Error] Wrong ${executable} version${NC}"
+    print_update_error
+  fi
+  echo "${path}"
+}
+
+readonly MYPY_PATH=$(find_executable mypy 1.5.1)
+
+echo
+echo "Style checking Python with mypy ..."
+"${MYPY_PATH}" --config-file=mypy.ini --check ./src

starter_project/autonomy/mypy.ini

@@ -0,0 +1 @@
+../../mypy.ini