diff --git a/.github/ISSUE_TEMPLATE/navigator.yaml b/.github/ISSUE_TEMPLATE/navigator.yaml index 8e528b706..a3617fae6 100644 --- a/.github/ISSUE_TEMPLATE/navigator.yaml +++ b/.github/ISSUE_TEMPLATE/navigator.yaml @@ -7,26 +7,6 @@ body: attributes: value: | Hey, thanks for taking the time to add this software issue! - - type: input - id: assignee - attributes: - label: Assignee - description: > - Does this issue need to be completed by a specific person on the team? - If so, mention them below. - placeholder: ex. @torvalds is already working on this task! - validations: - required: false - - type: input - id: deadline - attributes: - label: Deadline - description: > - Does this task need a deadline? _If this task is related to a competition, - it should have a deadline._ - placeholder: ex. 2023-12-25 - validations: - required: false - type: textarea id: description attributes: diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index c69ca062a..de7c09a7a 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -31,19 +31,20 @@ jobs: - name: Configure catkin workspace folder structure run: | mkdir -p $GITHUB_WORKSPACE/catkin_ws/src + sudo apt reinstall python3-pip - name: Check out code from GitHub uses: actions/checkout@v3.0.2 with: submodules: recursive path: catkin_ws/src/mil - name: Setup ROS Noetic - uses: ros-tooling/setup-ros@v0.3 + uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: noetic - name: Install pip dependencies run: | cd $GITHUB_WORKSPACE/catkin_ws/src/mil - pip install -r requirements.txt + pip3 install -r requirements.txt # We want to run a full test suite in CI - this includes the BlueView # tests! - name: Install BlueView Sonar SDK @@ -120,7 +121,7 @@ jobs: # We need to install rsync for GitHub Pages deploy action - name: Install rsync run: | - sudo apt-get upgrade && sudo apt-get update && sudo apt-get install -y rsync + sudo apt-get update && sudo apt-get upgrade && sudo apt-get install -y rsync # Publish the artifact to the GitHub Pages branch - name: Push docs to GitHub Pages diff --git a/.github/workflows/gh_pages.yaml b/.github/workflows/gh_pages.yaml index b54342c28..39b70c62a 100644 --- a/.github/workflows/gh_pages.yaml +++ b/.github/workflows/gh_pages.yaml @@ -20,7 +20,7 @@ jobs: # Wait for the CI to finish so we can download the docs artifact - name: Wait for CI - uses: lewagon/wait-on-check-action@v1.0.0 + uses: lewagon/wait-on-check-action@v1.3.3 if: github.event.action != 'closed' with: ref: ${{ github.event.pull_request.head.ref }} diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 93b2c6fc4..79a11871a 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -11,41 +11,41 @@ ci: repos: - repo: https://github.com/adrienverge/yamllint.git - rev: v1.30.0 + rev: v1.33.0 hooks: - id: yamllint - repo: https://github.com/psf/black - rev: 23.3.0 + rev: 24.1.1 hooks: - id: black - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v16.0.0 + rev: v17.0.6 hooks: - id: clang-format - repo: https://github.com/PyCQA/autoflake - rev: v2.0.2 + rev: v2.2.1 hooks: - id: autoflake args: [--remove-all-unused-imports, --ignore-init-module-imports] - repo: https://github.com/shellcheck-py/shellcheck-py - rev: v0.9.0.2 + rev: v0.9.0.6 hooks: - id: shellcheck exclude: ^docker|deprecated|NaviGator/simulation/VRX args: [--severity=warning, --exclude=SC1090] - repo: https://github.com/scop/pre-commit-shfmt - rev: v3.6.0-2 + rev: v3.7.0-4 hooks: - id: shfmt exclude: ^docker|deprecated|NaviGator/simulation/VRX - - repo: https://github.com/charliermarsh/ruff-pre-commit + - repo: https://github.com/astral-sh/ruff-pre-commit # Ruff version. - rev: 'v0.0.260' + rev: 'v0.2.0' hooks: - id: ruff args: [--fix, --exit-non-zero-on-fix] - repo: https://github.com/codespell-project/codespell - rev: v2.2.4 + rev: v2.2.6 hooks: - id: codespell args: @@ -73,11 +73,12 @@ repos: - id: prettier-package-xml - id: sort-package-xml - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.4.0 + rev: v4.5.0 hooks: - id: check-added-large-files - id: check-case-conflict - id: check-merge-conflict + exclude_types: [markdown, rst] - id: check-executables-have-shebangs - id: check-symlinks - id: check-json diff --git a/NaviGator/gnc/navigator_msg_multiplexer/nodes/ogrid_arbiter.py b/NaviGator/gnc/navigator_msg_multiplexer/nodes/ogrid_arbiter.py index ac8fffd4f..eb425ec7a 100755 --- a/NaviGator/gnc/navigator_msg_multiplexer/nodes/ogrid_arbiter.py +++ b/NaviGator/gnc/navigator_msg_multiplexer/nodes/ogrid_arbiter.py @@ -493,10 +493,7 @@ def publish(self, timer_event): ) print(e) fprint( - "w: {}, h: {}".format( - global_ogrid.info.width, - global_ogrid.info.height, - ), + f"w: {global_ogrid.info.width}, h: {global_ogrid.info.height}", msg_color="red", ) diff --git a/NaviGator/gnc/navigator_path_planner/nodes/path_planner.py b/NaviGator/gnc/navigator_path_planner/nodes/path_planner.py index ba33ca850..1f6ff6793 100755 --- a/NaviGator/gnc/navigator_path_planner/nodes/path_planner.py +++ b/NaviGator/gnc/navigator_path_planner/nodes/path_planner.py @@ -1070,7 +1070,7 @@ def reevaluate_plan(self) -> None: def action_check(self, _: rospy.timer.TimerEvent) -> None: """ Manages action preempting. Serves as the callback to a Timer operating - opereating every self.revisit_period seconds. + operating every self.revisit_period seconds. """ if self.preempted or not self.move_server.is_active(): return diff --git a/NaviGator/gnc/navigator_thrust_mapper/navigator_thrust_mapper/thruster_map.py b/NaviGator/gnc/navigator_thrust_mapper/navigator_thrust_mapper/thruster_map.py index 588679455..2fc19700a 100644 --- a/NaviGator/gnc/navigator_thrust_mapper/navigator_thrust_mapper/thruster_map.py +++ b/NaviGator/gnc/navigator_thrust_mapper/navigator_thrust_mapper/thruster_map.py @@ -166,15 +166,11 @@ def from_urdf( if find != -1 and find + len(transmission_suffix) == len(transmission.name): if len(transmission.joints) != 1: raise Exception( - "Transmission {} does not have 1 joint".format( - transmission.name, - ), + f"Transmission {transmission.name} does not have 1 joint", ) if len(transmission.actuators) != 1: raise Exception( - "Transmission {} does not have 1 actuator".format( - transmission.name, - ), + f"Transmission {transmission.name} does not have 1 actuator", ) t_ratio = transmission.actuators[0].mechanicalReduction @@ -195,9 +191,7 @@ def from_urdf( joint = t_joint if joint is None: rospy.logerr( - "Transmission joint {} not found".format( - transmission.joints[0].name, - ), + f"Transmission joint {transmission.joints[0].name} not found", ) try: trans = buff.lookup_transform( @@ -217,9 +211,7 @@ def from_urdf( joints.append(joint.name) if limit != -1 and joint.limit.effort != limit: raise Exception( - "Thruster {} had a different limit, cannot proceed".format( - joint.name, - ), + f"Thruster {joint.name} had a different limit, cannot proceed", ) limit = joint.limit.effort limit_tuple = (limit, -limit) diff --git a/NaviGator/hardware_drivers/navigator_kill_board/navigator_kill_board/constants.py b/NaviGator/hardware_drivers/navigator_kill_board/navigator_kill_board/constants.py index 83663e034..d5eaef7e4 100644 --- a/NaviGator/hardware_drivers/navigator_kill_board/navigator_kill_board/constants.py +++ b/NaviGator/hardware_drivers/navigator_kill_board/navigator_kill_board/constants.py @@ -17,6 +17,7 @@ The computer can also command a kill (for example, if ROS notices a criticaly low battery) by sending the COMPUTER.KILL.REQUEST and undone with COMPUTER.CLEAR.REQUEST """ + constants = { "TIMEOUT_SECONDS": 8.0, # How often board must be pinged to not set HEARTBERAT_REMOTE True # Note: not official documented, this is just a guess diff --git a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/battery_voltage.py b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/battery_voltage.py index 6c25b9169..c10b13c91 100644 --- a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/battery_voltage.py +++ b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/battery_voltage.py @@ -27,9 +27,9 @@ def _check_voltage(self, msg): if not self._raised or self._severity != severity: self.broadcaster.raise_alarm( severity=severity, - problem_description="battery critcaly low" - if severity == 2 - else "battery low", + problem_description=( + "battery critcaly low" if severity == 2 else "battery low" + ), parameters={"voltage": voltage}, ) diff --git a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/kill.py b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/kill.py index de42244da..547c56fa8 100644 --- a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/kill.py +++ b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/kill.py @@ -28,10 +28,7 @@ def _online_bagger_cb(self, status, result): rospy.loginfo(f"KILL BAG WRITTEN TO {result.filename}") else: rospy.logwarn( - "KILL BAG {}, status: {}".format( - TerminalState.to_string(status), - result.status, - ), + f"KILL BAG {TerminalState.to_string(status)}, status: {result.status}", ) def _kill_task_cb(self, status, result): @@ -39,10 +36,7 @@ def _kill_task_cb(self, status, result): rospy.loginfo("Killed task success!") return rospy.logwarn( - "Killed task failed ({}): {}".format( - TerminalState.to_string(status), - result.result, - ), + f"Killed task failed ({TerminalState.to_string(status)}): {result.result}", ) def raised(self, alarm): @@ -54,7 +48,7 @@ def raised(self, alarm): rospy.logwarn("BAG_ALWAYS or BAG_KILL not set. Not making kill bag.") else: goal = BagOnlineGoal(bag_name="kill.bag") - goal.topics = os.environ["BAG_ALWAYS"] + " " + os.environ["bag_kill"] + goal.topics = os.environ["BAG_ALWAYS"] + " " + os.environ["BAG_KILL"] self.bag_client.send_goal(goal, done_cb=self._online_bagger_cb) self.task_client.run_mission("Killed", done_cb=self._kill_task_cb) diff --git a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/odom_kill.py b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/odom_kill.py index 2bc5e6c43..d0fa63afd 100755 --- a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/odom_kill.py +++ b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/odom_kill.py @@ -51,9 +51,7 @@ def check_continuity(self, odom): self._raised = True # Avoid raising multiple times rospy.logwarn("ODOM DISCONTINUITY DETECTED") self.ab.raise_alarm( - problem_description="ODOM DISCONTINUITY DETECTED. JUMPED {} METERS".format( - jump, - ), + problem_description=f"ODOM DISCONTINUITY DETECTED. JUMPED {jump} METERS", severity=5, ) self.last_position = position diff --git a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/thruster_fault.py b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/thruster_fault.py index a89612632..ce89ac3a0 100644 --- a/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/thruster_fault.py +++ b/NaviGator/mission_control/navigator_alarm/navigator_alarm_handlers/thruster_fault.py @@ -65,9 +65,7 @@ def _check_faults(self, msg, topic): return self.broadcaster.raise_alarm( severity=5, - problem_description="{} thrusters have faults".format( - len(self._raised_alarms), - ), + problem_description=f"{len(self._raised_alarms)} thrusters have faults", parameters={ t: self._get_fault_codes(k) for t, k in self._raised_alarms.items() }, diff --git a/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_master.launch b/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_master.launch index 944df5bfa..377f824de 100644 --- a/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_master.launch +++ b/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_master.launch @@ -7,6 +7,7 @@ + @@ -30,5 +31,4 @@ - diff --git a/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_tf.launch b/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_tf.launch index f46bcca90..a9bdd66e8 100644 --- a/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_tf.launch +++ b/NaviGator/mission_control/navigator_launch/launch/vrx/vrx_tf.launch @@ -1,5 +1,8 @@ + + true + @@ -11,9 +14,10 @@ - - - - + + + + + diff --git a/NaviGator/mission_control/navigator_missions/launch/mission_params.yaml b/NaviGator/mission_control/navigator_missions/launch/mission_params.yaml index 7b44edc7e..c4ee5940c 100644 --- a/NaviGator/mission_control/navigator_missions/launch/mission_params.yaml +++ b/NaviGator/mission_control/navigator_missions/launch/mission_params.yaml @@ -157,14 +157,14 @@ dock_shape_1: param: /mission/identify_dock/shape_1 options: [CIRCLE, TRIANGLE, CROSS, ANY] description: > - The shape of the first bay to dock in during the dentify + The shape of the first bay to dock in during the identify Symbols and Dock mission. If set to ANY, mission should dock based on the color parameter and set this parameter during the mission dock_color_1: param: /mission/identify_dock/color_1 options: [RED, GREEN, BLUE, ANY] description: > - The color of the first bay to dock in during the dentify Symbols and Dock + The color of the first bay to dock in during the identify Symbols and Dock mission. If set to ANY, mission should dock based on the shape parameter and set this parameter during the mission @@ -174,7 +174,7 @@ dock_shape_2: description: > # yamllint disable-line rule:line-length The shape of the second bay to dock in during the - dentify Symbols and Dock mission. If set to ANY, + identify Symbols and Dock mission. If set to ANY, mission should dock based on the color parameter and set this parameter during the mission dock_color_2: @@ -183,6 +183,6 @@ dock_color_2: description: > # yamllint disable-line rule:line-length The color of the second bay to dock in during the - dentify Symbols and Dock mission. + identify Symbols and Dock mission. If set to ANY, mission should dock based on the shape parameter and set this parameter during the mission diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/constant_velocity.py b/NaviGator/mission_control/navigator_missions/navigator_missions/constant_velocity.py index c9a5a503b..a0ce42a26 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/constant_velocity.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/constant_velocity.py @@ -61,12 +61,12 @@ def decode_parameters(cls, parameters): if not isinstance(parsed, list) or len(parsed) != 3: raise err for i in range(3): - if not (isinstance(parsed[i], int) or isinstance(parsed[i], float)): + if not (isinstance(parsed[i], (int, float))): raise err return parsed async def run(self, args): - # Publish a velocity of zero for a while to stabalize navigator + # Publish a velocity of zero for a while to stabilize navigator self.send_feedback("Switching trajectory to constant") await self.change_trajectory("constant") await self.nh.sleep(0.1) diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/demonstrate_navigation.py b/NaviGator/mission_control/navigator_missions/navigator_missions/demonstrate_navigation.py index 6c53d35a4..599a388cc 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/demonstrate_navigation.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/demonstrate_navigation.py @@ -68,7 +68,7 @@ async def run(self, parameters): _, closest_reds = await self.get_sorted_objects("red_cylinder", 1) _, closest_greens = await self.get_sorted_objects("green_cylinder", 1) - # Rename the totems for their symantic name + # Rename the totems for their semantic name green_close = closest_greens[0] red_close = closest_reds[0] @@ -83,7 +83,7 @@ async def run(self, parameters): _, closest_reds = await self.get_sorted_objects("red_cylinder", 2) _, closest_greens = await self.get_sorted_objects("green_cylinder", 2) - # Rename the totems for their symantic name + # Rename the totems for their semantic name green_far = closest_greens[1] red_far = closest_reds[1] diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/detect_deliver_2016.py b/NaviGator/mission_control/navigator_missions/navigator_missions/detect_deliver_2016.py index bc95612b0..dee540de0 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/detect_deliver_2016.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/detect_deliver_2016.py @@ -153,9 +153,7 @@ async def do_circle(): await self.nh.sleep(0.25) continue fprint( - "Shape ({}found, using normal to look at other 3 shapes if needed".format( - res[0], - ), + f"Shape ({res[0]}found, using normal to look at other 3 shapes if needed", title="DETECT DELIVER", msg_color="green", ) @@ -240,10 +238,7 @@ async def search_sides(self, moves): self.shape_pose = found_pose return fprint( - "Saw (Shape={}, Color={}) on this side".format( - shape_color[0], - shape_color[1], - ), + f"Saw (Shape={shape_color[0]}, Color={shape_color[1]}) on this side", title="DETECT DELIVER", msg_color="green", ) @@ -287,10 +282,7 @@ def select_backup_shape(self): self.shape_pose = point_normal if self.Shape == shape or self.Color == color: fprint( - "Correct shape not found, resorting to shape={} color={}".format( - shape, - color, - ), + f"Correct shape not found, resorting to shape={shape} color={color}", title="DETECT DELIVER", msg_color="yellow", ) @@ -430,9 +422,7 @@ async def shoot_and_align_forest(self): move = await self.align_to_target() if move.failure_reason != "": fprint( - "Error Aligning with target = {}. Ending mission :(".format( - move.failure_reason, - ), + f"Error Aligning with target = {move.failure_reason}. Ending mission :(", title="DETECT DELIVER", msg_color="red", ) @@ -479,9 +469,7 @@ async def shoot_and_align(self): move = await self.align_to_target() if move.failure_reason != "": fprint( - "Error Aligning with target = {}. Ending mission :(".format( - move.failure_reason, - ), + f"Error Aligning with target = {move.failure_reason}. Ending mission :(", title="DETECT DELIVER", msg_color="red", ) diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/identify_dock.py b/NaviGator/mission_control/navigator_missions/navigator_missions/identify_dock.py index 28419f6b6..6d2203826 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/identify_dock.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/identify_dock.py @@ -150,10 +150,7 @@ async def check_bays(self): def update_shape(self, shape_res, normal_res, tf): print_good( - "Found (Shape={}, Color={} in a bay".format( - shape_res.Shape, - shape_res.Color, - ), + f"Found (Shape={shape_res.Shape}, Color={shape_res.Color} in a bay", ) self.identified_shapes[(shape_res.Shape, shape_res.Color)] = self.get_shape_pos( normal_res, diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/move.py b/NaviGator/mission_control/navigator_missions/navigator_missions/move.py index 03017c09b..a9debd213 100755 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/move.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/move.py @@ -149,9 +149,7 @@ async def run(self, args): "yl": "yaw_left", "yr": "yaw_right", } - command = ( - command if command not in shorthand.keys() else shorthand[command] - ) + command = shorthand.get(command, command) movement = getattr(self.move, command) trans_move = command[:3] != "yaw" diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/navigation.py b/NaviGator/mission_control/navigator_missions/navigator_missions/navigation.py index 9b977b0bf..7812e5108 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/navigation.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/navigation.py @@ -187,9 +187,7 @@ async def explore_closest_until(self, is_done, filter_and_sort): ) if classification_index != -1: self.send_feedback( - "{} identified. Canceling investigation".format( - move_id_tuple[1], - ), + f"{move_id_tuple[1]} identified. Canceling investigation", ) move_task.cancel() diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/pose_editor.py b/NaviGator/mission_control/navigator_missions/navigator_missions/pose_editor.py index 5f25f36ab..d2b1855cd 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/pose_editor.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/pose_editor.py @@ -341,9 +341,7 @@ def as_MoveGoal(self, move_type=MoveGoal.DRIVE, **kwargs): for key in kwargs: if not hasattr(MoveGoal, key): fprint( - "MoveGoal msg doesn't have a field called '{}' you tried to set via kwargs.".format( - key, - ), + f"MoveGoal msg doesn't have a field called '{key}' you tried to set via kwargs.", title="POSE_EDITOR", msg_color="red", ) diff --git a/NaviGator/mission_control/navigator_missions/vrx_missions/dock.py b/NaviGator/mission_control/navigator_missions/vrx_missions/dock.py index 23d7cdfb3..44a3cacae 100644 --- a/NaviGator/mission_control/navigator_missions/vrx_missions/dock.py +++ b/NaviGator/mission_control/navigator_missions/vrx_missions/dock.py @@ -607,7 +607,7 @@ async def prepare_for_docking(self): # This function looks at the two squares in front of the boat # and it gets the middle pixel between the two squares. # If the middle pixel is for some reason not in the middle of our camera... - # adjust the boat postiion before docking + # adjust the boat position before docking print("prepare for landing!") img = await self.front_left_camera_sub.get_next_message() diff --git a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx.py b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx.py index d629df644..91514c01d 100644 --- a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx.py +++ b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx.py @@ -29,145 +29,143 @@ class Vrx(NaviGatorMission): nh: NodeHandle - def __init__(self, *args, **kwargs): - super().__init__(*args, **kwargs) - - @staticmethod - async def init(): - if hasattr(Vrx, "_vrx_init"): + @classmethod + async def setup(cls): + if hasattr(cls, "_cls_init"): return - Vrx.from_lla = Vrx.nh.get_service_client("/fromLL", FromLL) - Vrx.to_lla = Vrx.nh.get_service_client("/toLL", ToLL) - Vrx.task_info_sub = Vrx.nh.subscribe("/vrx/task/info", Task) - await Vrx.task_info_sub.setup() - Vrx.scan_dock_color_sequence = Vrx.nh.get_service_client( + cls.from_lla = cls.nh.get_service_client("/fromLL", FromLL) + cls.to_lla = cls.nh.get_service_client("/toLL", ToLL) + cls.task_info_sub = cls.nh.subscribe("/vrx/task/info", Task) + cls.scan_dock_color_sequence = cls.nh.get_service_client( "/vrx/scan_dock_deliver/color_sequence", ColorSequence, ) - Vrx.fire_ball = Vrx.nh.advertise("/wamv/shooters/ball_shooter/fire", Empty) - Vrx.station_keep_goal = Vrx.nh.subscribe( + cls.fire_ball = cls.nh.advertise("/wamv/shooters/ball_shooter/fire", Empty) + cls.station_keep_goal = cls.nh.subscribe( "/vrx/station_keeping/goal", GeoPoseStamped, ) - Vrx.wayfinding_path_sub = Vrx.nh.subscribe("/vrx/wayfinding/waypoints", GeoPath) - Vrx.station_keeping_pose_error = Vrx.nh.subscribe( + cls.wayfinding_path_sub = cls.nh.subscribe("/vrx/wayfinding/waypoints", GeoPath) + cls.station_keeping_pose_error = cls.nh.subscribe( "/vrx/station_keeping/pose_error", Float64, ) - Vrx.station_keeping_rms_error = Vrx.nh.subscribe( + cls.station_keeping_rms_error = cls.nh.subscribe( "/vrx/station_keeping/rms_error", Float64, ) - Vrx.wayfinding_min_errors = Vrx.nh.subscribe( + cls.wayfinding_min_errors = cls.nh.subscribe( "/vrx/wayfinding/min_errors", Float64MultiArray, ) - Vrx.wayfinding_mean_error = Vrx.nh.subscribe( + cls.wayfinding_mean_error = cls.nh.subscribe( "/vrx/wayfinding/mean_error", Float64, ) - Vrx.perception_landmark = Vrx.nh.advertise( + cls.perception_landmark = cls.nh.advertise( "/vrx/perception/landmark", GeoPoseStamped, ) await asyncio.gather( - Vrx.fire_ball.setup(), - Vrx.station_keep_goal.setup(), - Vrx.wayfinding_path_sub.setup(), - Vrx.station_keeping_pose_error.setup(), - Vrx.station_keeping_rms_error.setup(), - Vrx.wayfinding_min_errors.setup(), - Vrx.wayfinding_mean_error.setup(), - Vrx.perception_landmark.setup(), + cls.task_info_sub.setup(), + cls.fire_ball.setup(), + cls.station_keep_goal.setup(), + cls.wayfinding_path_sub.setup(), + cls.station_keeping_pose_error.setup(), + cls.station_keeping_rms_error.setup(), + cls.wayfinding_min_errors.setup(), + cls.wayfinding_mean_error.setup(), + cls.perception_landmark.setup(), ) - Vrx.animal_landmarks = Vrx.nh.subscribe("/vrx/wildlife/animals/poses", GeoPath) - Vrx.beacon_landmark = Vrx.nh.get_service_client("beaconLocator", AcousticBeacon) - Vrx.circle_animal = Vrx.nh.get_service_client("/choose_animal", ChooseAnimal) - Vrx.set_long_waypoint = Vrx.nh.get_service_client( + cls.animal_landmarks = cls.nh.subscribe("/vrx/wildlife/animals/poses", GeoPath) + cls.beacon_landmark = cls.nh.get_service_client("beaconLocator", AcousticBeacon) + cls.circle_animal = cls.nh.get_service_client("/choose_animal", ChooseAnimal) + cls.set_long_waypoint = cls.nh.get_service_client( "/set_long_waypoint", MoveToWaypoint, ) - Vrx.yolo_objects = Vrx.nh.subscribe("/yolov7/detections", Detection2DArray) - Vrx.tf_listener = axros_tf.TransformListener(Vrx.nh) - await Vrx.tf_listener.setup() - Vrx.database_response = Vrx.nh.get_service_client( + cls.yolo_objects = cls.nh.subscribe("/yolov7/detections", Detection2DArray) + cls.tf_listener = axros_tf.TransformListener(cls.nh) + await cls.tf_listener.setup() + cls.database_response = cls.nh.get_service_client( "/database/requests", ObjectDBQuery, ) - Vrx.get_two_closest_cones = Vrx.nh.get_service_client( + cls.get_two_closest_cones = cls.nh.get_service_client( "/get_two_closest_cones", TwoClosestCones, ) await asyncio.gather( - Vrx.animal_landmarks.setup(), - Vrx.yolo_objects.setup(), + cls.animal_landmarks.setup(), + cls.yolo_objects.setup(), ) - Vrx.pcodar_reset = Vrx.nh.get_service_client("/pcodar/reset", Trigger) + cls.pcodar_reset = cls.nh.get_service_client("/pcodar/reset", Trigger) - Vrx.front_left_camera_info_sub = None - Vrx.front_left_camera_sub = None - Vrx.front_right_camera_info_sub = None - Vrx.front_right_camera_sub = None + cls.front_left_camera_info_sub = None + cls.front_left_camera_sub = None + cls.front_right_camera_info_sub = None + cls.front_right_camera_sub = None - Vrx._vrx_init = True + cls._cls_init = True - @staticmethod - async def shutdown(): + @classmethod + async def shutdown(cls): + return await asyncio.gather( - Vrx.task_info_sub.shutdown(), - Vrx.animal_landmarks.shutdown(), - Vrx.yolo_objects.shutdown(), - Vrx.fire_ball.shutdown(), - Vrx.station_keep_goal.shutdown(), - Vrx.wayfinding_path_sub.shutdown(), - Vrx.station_keeping_pose_error.shutdown(), - Vrx.station_keeping_rms_error.shutdown(), - Vrx.wayfinding_min_errors.shutdown(), - Vrx.wayfinding_mean_error.shutdown(), - Vrx.perception_landmark.shutdown(), + cls.task_info_sub.shutdown(), + cls.animal_landmarks.shutdown(), + cls.yolo_objects.shutdown(), + cls.fire_ball.shutdown(), + cls.station_keep_goal.shutdown(), + cls.wayfinding_path_sub.shutdown(), + cls.station_keeping_pose_error.shutdown(), + cls.station_keeping_rms_error.shutdown(), + cls.wayfinding_min_errors.shutdown(), + cls.wayfinding_mean_error.shutdown(), + cls.perception_landmark.shutdown(), ) def cleanup(self): pass - @staticmethod - async def init_front_left_camera(): - if Vrx.front_left_camera_sub is None: - Vrx.front_left_camera_sub = Vrx.nh.subscribe( + @classmethod + async def init_front_left_camera(cls): + if cls.front_left_camera_sub is None: + cls.front_left_camera_sub = cls.nh.subscribe( "/wamv/sensors/cameras/front_left_camera/image_raw", Image, ) - if Vrx.front_left_camera_info_sub is None: - Vrx.front_left_camera_info_sub = Vrx.nh.subscribe( + if cls.front_left_camera_info_sub is None: + cls.front_left_camera_info_sub = cls.nh.subscribe( "/wamv/sensors/cameras/front_left_camera/camera_info", CameraInfo, ) await asyncio.gather( - Vrx.front_left_camera_sub.setup(), - Vrx.front_left_camera_info_sub.setup(), + cls.front_left_camera_sub.setup(), + cls.front_left_camera_info_sub.setup(), ) - @staticmethod - async def init_front_right_camera(): - if Vrx.front_right_camera_sub is None: - Vrx.front_right_camera_sub = Vrx.nh.subscribe( + @classmethod + async def init_front_right_camera(cls): + if cls.front_right_camera_sub is None: + cls.front_right_camera_sub = cls.nh.subscribe( "/wamv/sensors/cameras/front_right_camera/image_raw", Image, ) - if Vrx.front_right_camera_info_sub is None: - Vrx.front_right_camera_info_sub = Vrx.nh.subscribe( + if cls.front_right_camera_info_sub is None: + cls.front_right_camera_info_sub = cls.nh.subscribe( "/wamv/sensors/cameras/front_right_camera/camera_info", CameraInfo, ) await asyncio.gather( - Vrx.front_right_camera_sub.setup(), - Vrx.front_right_camera_info_sub.setup(), + cls.front_right_camera_sub.setup(), + cls.front_right_camera_info_sub.setup(), ) async def geo_pose_to_enu_pose(self, geo): diff --git a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_navigation.py b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_navigation.py index 3086e39b5..7ef98c064 100644 --- a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_navigation.py +++ b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_navigation.py @@ -151,9 +151,7 @@ async def explore_closest_until(self, is_done, filter_and_sort): ) if classification_index != -1: self.send_feedback( - "{} identified. Canceling investigation".format( - move_id_tuple[1], - ), + f"{move_id_tuple[1]} identified. Canceling investigation", ) move_id_tuple[0].cancel() await self.nh.sleep(1.0) diff --git a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_perception.py b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_perception.py index f1483416f..2b1ef6d50 100644 --- a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_perception.py +++ b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_perception.py @@ -79,11 +79,7 @@ async def run(self, parameters): ) elif objects[key] != new_objects[key]: self.send_feedback( - "{} changed from {} to {}".format( - key, - objects[key], - new_objects[key], - ), + f"{key} changed from {objects[key]} to {new_objects[key]}", ) await self.announce_object( key, diff --git a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py index 60496d0b8..966f38be0 100644 --- a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py +++ b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py @@ -5,6 +5,7 @@ from .vrx import Vrx ___author___ = "Alex Perez" +# Optimized by Daniel Parra class VrxWayfinding2(Vrx): @@ -42,12 +43,18 @@ async def run(self, parameters): poses = poses[:start_pose_index] path = path[1:] - # self.send_feedback('Sorted poses' + str(poses)) await self.wait_for_task_such_that(lambda task: task.state in ["running"]) # do movements for index in path: self.send_feedback(f"Going to {poses[index]}") - # Go to goal + P = 0.85 + part_way_point = [x * P for x in poses[index][0][:-1]] + part_way_point.append(poses[index][0][-1]) + self.send_feedback( + f"\nPartway:\n{part_way_point}\nEndPoint:\n{poses[index][0]}", + ) + + await self.send_trajectory_without_path([part_way_point, poses[index][1]]) await self.send_trajectory_without_path(poses[index]) diff --git a/NaviGator/satellite/rviz_satellite/src/aerialmap_display.cpp b/NaviGator/satellite/rviz_satellite/src/aerialmap_display.cpp index 22a307f76..bf5ec8b20 100644 --- a/NaviGator/satellite/rviz_satellite/src/aerialmap_display.cpp +++ b/NaviGator/satellite/rviz_satellite/src/aerialmap_display.cpp @@ -416,7 +416,7 @@ void AerialMapDisplay::assembleScene() // determine location of this tile, flipping y in the process const double x = (tile.x() - loader_->centerTileX()) * tile_w + origin_x; const double y = -(tile.y() - loader_->centerTileY()) * tile_h + origin_y; - // don't re-use any ids + // don't reuse any ids const std::string name_suffix = std::to_string(tile.x()) + "_" + std::to_string(tile.y()) + "_" + std::to_string(map_id_) + "_" + std::to_string(scene_id_); diff --git a/NaviGator/scripts/bash_aliases.sh b/NaviGator/scripts/bash_aliases.sh index 216b1ff80..de6da5f4b 100755 --- a/NaviGator/scripts/bash_aliases.sh +++ b/NaviGator/scripts/bash_aliases.sh @@ -18,7 +18,7 @@ nthrust() { topic="/$1_motor/cmd" publishers=$(rostopic info "$topic" | grep Publishers) if [ "$publishers" != "Publishers: None" ]; then - echo "Somone is already publishing to $topic. Perhaps you need to kill thrust mapper?" + echo "Someone is already publishing to $topic. Perhaps you need to kill thrust mapper?" return 1 fi rostopic pub "$topic" "roboteq_msgs/Command" "setpoint: $2" -r100 diff --git a/NaviGator/simulation/navigator_gazebo/test/test_sim_integration.py b/NaviGator/simulation/navigator_gazebo/test/test_sim_integration.py index 3cb7ab8a7..46e20b856 100755 --- a/NaviGator/simulation/navigator_gazebo/test/test_sim_integration.py +++ b/NaviGator/simulation/navigator_gazebo/test/test_sim_integration.py @@ -71,10 +71,7 @@ def test_odom(self): rospy.sleep(0.1) self.assertTrue( len(self.odom_pos_msg) == 3 and len(self.odom_ori_msg) == 4, - msg="POS, ORI: {}, {}".format( - len(self.odom_pos_msg), - len(self.odom_ori_msg), - ), + msg=f"POS, ORI: {len(self.odom_pos_msg)}, {len(self.odom_ori_msg)}", ) initial_pos = [-1.2319, 0.0, 0.0] initial_ori = [0.0, 0.0, 0.0, 1.0] @@ -100,10 +97,7 @@ def test_absodom(self): rospy.sleep(0.1) self.assertTrue( len(self.absodom_pos_msg) == 3 and len(self.absodom_ori_msg) == 4, - msg="POS, ORI: {}, {}".format( - len(self.absodom_pos_msg), - len(self.absodom_ori_msg), - ), + msg=f"POS, ORI: {len(self.absodom_pos_msg)}, {len(self.absodom_ori_msg)}", ) initial_pos = [743789.637462, -5503821.36715, 3125622.10477] initial_ori = [0.0, 0.0, 0.0, 1.0] @@ -122,25 +116,13 @@ def verify_pos_ori(self, pos, initial_pos, ori, initial_ori, topic): actual, initial, places=0, - msg=( - "Error: {} position is: {} should be {}".format( - topic, - actual, - initial, - ) - ), + msg=(f"Error: {topic} position is: {actual} should be {initial}"), ) for actual, initial in zip(ori, initial_ori): self.assertEqual( actual, initial, - msg=( - "Error: {} orientation is: {} should be {}".format( - topic, - actual, - initial, - ) - ), + msg=(f"Error: {topic} orientation is: {actual} should be {initial}"), ) def cam_info_right_cb(self, msg): @@ -229,10 +211,7 @@ def verify_not_empty(self, data_lists, num_topics): self.assertEqual( len(data_lists), num_topics, - msg="Number of topics is {}, should be {}".format( - len(data_lists), - num_topics, - ), + msg=f"Number of topics is {len(data_lists)}, should be {num_topics}", ) for data_list in data_lists: self.assertNotEqual(len(data_list), 0, msg="data is empty") @@ -243,13 +222,7 @@ def verify_info(self, res_info, initial_info, topic): self.assertNotEqual( actual_dim, initial_dim, - msg=( - "Error: {} is: {} shouldn't be {}".format( - topic, - actual_dim, - initial_dim, - ) - ), + msg=(f"Error: {topic} is: {actual_dim} shouldn't be {initial_dim}"), ) diff --git a/NaviGator/utils/navigator_msgs/srv/GetDockBays.srv b/NaviGator/utils/navigator_msgs/srv/GetDockBays.srv index 9a39764bd..7081b1b07 100644 --- a/NaviGator/utils/navigator_msgs/srv/GetDockBays.srv +++ b/NaviGator/utils/navigator_msgs/srv/GetDockBays.srv @@ -2,4 +2,4 @@ geometry_msgs/Point[3] bays #The positions in ENU frame of the center of the three bays 0=left, 1=center, 2=right geometry_msgs/Vector3 normal #Vector or normal pointing away from plane of dock back bool success #False if an error occurred getting dock bays -string error #Descripion of error if success=false +string error #Description of error if success=false diff --git a/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py b/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py index d8583f010..59e6139e3 100755 --- a/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py +++ b/NaviGator/utils/navigator_robotx_comms/nodes/robotx_comms_client.py @@ -559,10 +559,7 @@ def connect(self) -> None: """ if not self.connected: rospy.loginfo( - "Attempting Connection to TD Server at {}:{}".format( - self.tcp_ip, - self.tcp_port, - ), + f"Attempting Connection to TD Server at {self.tcp_ip}:{self.tcp_port}", ) while not self.connected and not rospy.is_shutdown(): # recreate socket diff --git a/NaviGator/utils/navigator_tools/navigator_tools/object_database_helper.py b/NaviGator/utils/navigator_tools/navigator_tools/object_database_helper.py index 0798c78e0..5cb8762cb 100644 --- a/NaviGator/utils/navigator_tools/navigator_tools/object_database_helper.py +++ b/NaviGator/utils/navigator_tools/navigator_tools/object_database_helper.py @@ -1,4 +1,3 @@ -"""Use the DBHelper class to interface with the Database without having to deal with ROS things.""" import asyncio import sys import time @@ -14,10 +13,29 @@ class DBHelper: - """DBHelper class.""" + """ + Use the DBHelper class to interface with the Database without having to deal with ROS things. + + Attributes: + found (bool): checks whether the object is what the user has been looking for. + nh (axros.NodeHandle): processes database requests. + position (int): the current position of the object. + rot (): one of the dimensional values. + new_object_subscriber (): an object that is being called from the database. + ensuring_objects (bool): a variable state for the object. + ensuring_object_dep (): a list of objects being used for storage. + ensuring_object_cb (): this is considered when it is an "ensuring_objects". + looking_for (): a setter variable to which a name is being assigned. + is_found (bool): determines whether the object is found or not. + """ def __init__(self, nh): - """Initialize the DB helper class.""" + """ + Initialize the DB helper class. + + Args: + nh(NodeHandle): NodeHandle object + """ self.found = set() self.nh = nh self.position = None @@ -30,7 +48,12 @@ def __init__(self, nh): self.is_found = False async def init_(self, navigator=None): - """Initialize the axros parts of the DBHelper.""" + """ + Initialize the axros parts of the DBHelper. + + Args: + navigator (NaviGator | None): Base NaviGator object + """ # self._sub_database = yield self.nh.subscribe('/database/objects', PerceptionObjectArray, self.object_cb) self._database = self.nh.get_service_client("/database/requests", ObjectDBQuery) self.navigator = navigator @@ -43,21 +66,37 @@ async def init_(self, navigator=None): return self def _odom_cb(self, odom): + """ + Sets the position and the rot values. + + Args: + odom (object): Odom + """ self.position, self.rot = nt.odometry_to_numpy(odom)[0] async def get_object_by_id(self, my_id): + """ + Retrieves the object with the associated "my_id". + + Args: + my_id (int): an "id" of the object is passed in. + + Returns: + An individual object is being returned with a unique id. + """ print(my_id) req = ObjectDBQueryRequest() req.name = "all" resp = await self._database(req) - ans = [obj for obj in resp.objects if obj.id == my_id][0] + ans = next(obj for obj in resp.objects if obj.id == my_id) return ans async def begin_observing(self, cb): """ Get notified when a new object is added to the database. - cb : The callback that is called when a new object is added to the database + Args: + cb : The callback that is called when a new object is added to the database. """ self.new_object_subscriber = cb req = ObjectDBQueryRequest() @@ -78,6 +117,12 @@ async def begin_observing(self, cb): self.new_object_subscriber(o) async def get_unknown_and_low_conf(self): + """ + Gets unknown objects that are of low confidence. + + Returns: + Objects that meet certain criteria. + """ req = ObjectDBQueryRequest() req.name = "all" resp = await self._database(req) @@ -91,9 +136,21 @@ async def get_unknown_and_low_conf(self): return m def set_looking_for(self, name): + """ + Setting to a name that is being looked for. + + Args: + name (string): name of the object. + """ self.looking_for = name - def is_found_func(self): + def is_found_func(self) -> bool: + """ + Determines whether the object is being found or not. + + Returns: + The existence of the object is returned. + """ if self.is_found: self.looking_for = None self.is_found = False @@ -101,7 +158,12 @@ def is_found_func(self): return False def object_cb(self, perception_objects): - """Callback for the object database.""" + """ + Callback for the object database. + + Args: + perception_objects (object): a list of objects that are passed. + """ self.total_num = len(perception_objects.objects) for o in perception_objects.objects: if o.name == self.looking_for: @@ -120,12 +182,23 @@ def object_cb(self, perception_objects): self.ensuring_object_cb(missings_objs) def remove_found(self, name): - """Remove an object that has been listed as found.""" + """ + Remove an object that has been listed as found. + + Args: + name (string): a name of the object is being passed in. + """ self.found.remove(name) def ensure_object_permanence(self, object_dep, cb): - """Ensure that all the objects in the object_dep list remain in the database. - Call the callback if this isn't true.""" + """ + Ensure that all the objects in the object_dep list remain in the database. + Call the callback if this isn't true. + + Args: + object_dep: a new object is passed in order to be set. + cb : The callback that is called when a new object is added to the database. + """ if object_dep is None or cb is None: return self.ensuring_objects = True @@ -133,10 +206,21 @@ def ensure_object_permanence(self, object_dep, cb): self.ensuring_object_dep = object_dep def stop_ensuring_object_permanence(self): - """Stop ensuring that objects remain in the database.""" + """ + Stop ensuring that objects remain in the database. + """ self.ensuring_objects = False - def _wait_for_position(self, timeout=10): + def _wait_for_position(self, timeout=10) -> bool: + """ + A possible position is being stored in a variable. + + Args: + timeout (int): time limit to retrieve something. + + Returns: + bool: Determines whether the position is found within the time limit. + """ count = 0 while self.position is None: if self.navigator is not None: @@ -148,7 +232,15 @@ def _wait_for_position(self, timeout=10): return True async def get_closest_object(self, objects): - """Get the closest mission.""" + """ + Get the closest mission. + + Args: + objects (object): a list of objects are being passed in. + + Returns: + A object with the closest mission / distance is returned. + """ pobjs = [] for obj in objects: req = ObjectDBQueryRequest() @@ -171,6 +263,15 @@ async def get_closest_object(self, objects): return min_obj async def _dist(self, x): + """ + Finds the distance of the object keeping into account its current position. + + Args: + x (object): a specific object is being passed in order to retrieve its position. + + Returns: + the distance between the two objects is being returned. + """ if self.position is None: success = asyncio.create_task(self._wait_for_position) if not success: @@ -188,7 +289,18 @@ async def get_object( thresh=50, thresh_strict=50, ): - """Get an object from the database.""" + """ + Get an object from the database. + + Args: + object_name (string): the name of the object is passed in. + volume_only (bool): determines whether it is volume-based or not. + thresh (int): a maximum distance is passed in. + thresh_strict(int): a more strict maximum distance is passed in. + + Returns: + closest_potential_object (object): returns the object that is within the closest range. + """ if volume_only: req = ObjectDBQueryRequest() req.name = object_name @@ -233,6 +345,15 @@ async def get_object( return closest_potential_object def wait_for_additional_objects(self, timeout=60): + """ + Returns true/false whether additional objects are being passed in. + + Args: + timeout (int): maximum time limit of 60 seconds to run the program. + + Returns: + (bool): determines whether additional objects are added. + """ num_items = self.num_items start = time.time() while timeout < time.time() - start: @@ -241,14 +362,36 @@ def wait_for_additional_objects(self, timeout=60): return False def set_color(self, color, name): - """Set the color of an object in the database.""" + """ + Sets the color of an object in the database. + + Args: + color: the new color of the object is passed in. + name: the name of the object is passed in. + """ raise NotImplementedError def set_fake_position(self, pos): - """Set the position of a fake perception object.""" + """ + Sets the position of a fake perception object. + + Args: + pos (int): the new position of the object is passed in. + """ raise NotImplementedError async def get_objects_in_radius(self, pos, radius, objects="all"): + """ + Retrieves the objects that are present within the radius. + + Args: + pos (int): the position of the object is passed in. + radius (int): the radius of the object is passed in. + objects (object): this is a list of all the objects that needs to be compared. + + Returns: + ans (object): returns the objects that are present within the radius. + """ req = ObjectDBQueryRequest() req.name = "all" resp = await self._database(req) diff --git a/NaviGator/utils/navigator_tools/nodes/estimated_object_setter.py b/NaviGator/utils/navigator_tools/nodes/estimated_object_setter.py index 99845b004..57d4b7c01 100755 --- a/NaviGator/utils/navigator_tools/nodes/estimated_object_setter.py +++ b/NaviGator/utils/navigator_tools/nodes/estimated_object_setter.py @@ -24,7 +24,7 @@ async def main(name, lla): name = "_".join(txt.title() for txt in name.split("_")) point = await convert.request(CoordinateConversionRequest(frame="lla", point=lla)) - await db(ObjectDBQueryRequest(cmd="{}={p[0]}, {p[1]}".format(name, p=point.enu))) + await db(ObjectDBQueryRequest(cmd=f"{name}={point.enu[0]}, {point.enu[1]}")) if __name__ == "__main__": diff --git a/NaviGator/utils/navigator_tools/nodes/navigator_status_tui b/NaviGator/utils/navigator_tools/nodes/navigator_status_tui index 3efa0a5d1..0fc39eb3b 100755 --- a/NaviGator/utils/navigator_tools/nodes/navigator_status_tui +++ b/NaviGator/utils/navigator_tools/nodes/navigator_status_tui @@ -35,7 +35,7 @@ class nav_tui: self.panel.hide() panel.update_panels() self.rate = rospy.Rate(2) # Fixed rate for while loop to update at: 2 hz - # The following are default field initalizations for various values that that will be overwritten + # The following are default field initializations for various values that that will be overwritten self.voltage = 0 self.wrench = None self.LAT = None @@ -214,15 +214,13 @@ class nav_tui: if len(self.decode_fault_status(self.FL_fault)) == 0: self.window.addstr(5, self.x / 2 - 7, "No faults", curses.color_pair(3)) else: - n = 0 - for fault in self.decode_fault_status(self.FL_fault): + for n, fault in enumerate(self.decode_fault_status(self.FL_fault)): self.window.addstr( 5 + n, self.x / 2 - 7, "%s" % fault, curses.color_pair(1), ) - n += 1 self.window.addstr(14, self.x / 2 - 7, "Back Left", color) if len(self.decode_fault_status(self.BL_fault)) == 0: diff --git a/NaviGator/utils/remote_control/navigator_emergency_control/nodes/navigator_emergency.py b/NaviGator/utils/remote_control/navigator_emergency_control/nodes/navigator_emergency.py index 8077b4601..49b799fa4 100755 --- a/NaviGator/utils/remote_control/navigator_emergency_control/nodes/navigator_emergency.py +++ b/NaviGator/utils/remote_control/navigator_emergency_control/nodes/navigator_emergency.py @@ -67,8 +67,7 @@ def reset(self) -> None: def check_for_timeout(self, joy: Joy): """ - Consists of several procedures that reference parameters that are retrieved from the "Joy" object in - order to determine the state of the controller or whether it is a timeout phase. + This checks for a particular duration when the controller times out. Args: joy (Joy): The Joy message. @@ -95,7 +94,7 @@ def check_for_timeout(self, joy: Joy): def joy_recieved(self, joy: Joy) -> None: """ - Button elements are being assigned and simplied to readable names. The + Button elements are being assigned and simplified to readable names. The number of deployments or retractions for thrusters are being updated based on several conditions. Moreover, additional settings are changed based on the state of the controller and the activation of potential alarms or switches. diff --git a/NaviGator/utils/voltage_gui/src/voltage_gui.py b/NaviGator/utils/voltage_gui/src/voltage_gui.py index 91e5396e6..79e6a0e85 100644 --- a/NaviGator/utils/voltage_gui/src/voltage_gui.py +++ b/NaviGator/utils/voltage_gui/src/voltage_gui.py @@ -11,15 +11,33 @@ from roboteq_msgs.msg import Feedback from std_msgs.msg import Float32 -# Display voltage from battery_monitor and the four motors to a GUI - - __author__ = "Joseph Brooks" __email__ = "brooksturtle@ufl.edu" __license__ = "MIT" class voltageGUI(Plugin): + """ + Display voltage from battery_monitor and the four motors to a GUI + + Attributes: + height (int): defines the height of the screen + fontSize(int): sets the font size of the screen + warningCounter(int): prints the number of warning signs + paramCounter(int): the number of times "updateLabel" function is called + heightRatio(int): the ratio change in height that is caused by the resize + gotParams(bool): this checks to see whether the parameters are set + lowThreshold(int): colors values which include Good (Green), Warning (Yellow), and Critical (Red) + criticalThreshold(int): colors values which include Good (Green), Warning (Yellow), and Critical (Red) + paramCounter(int): number of parameters that are called by function + + battery_voltage(int): the amount of voltage that is stored in the battery + voltageFL(int): the voltage that is being supplied from dataFL + voltageFR(int): the voltage that is being supplied from dataFR + voltageBL(int): the voltage that is being supplied from dataBL + voltageBR(int): the voltage that is being supplied from dataBR + """ + def __init__(self, context): super().__init__(context) self.setObjectName("voltage_gui") @@ -33,8 +51,10 @@ def __init__(self, context): context.add_widget(self.myWidget) self.runGUI() - # Updates Values displayed in GUI every second def runGUI(self) -> None: + """ + Updates Values displayed in GUI every second + """ app = QApplication(sys.argv) self.myWidget.show() while 0 == 0: @@ -57,12 +77,12 @@ def __init__(self): # Whenever the screen is resized the resizeFont function is called self.resized.connect(self.resizeFont) - self.height = VoltageWidget.frameGeometry(self).height() + self.height = VoltageWidget.frameGeometry(self).height() # done - self.fontSize = 40 + self.fontSize = 40 # done - self.warningCounter = 0 - self.paramCounter = 0 + self.warningCounter = 0 # done + self.paramCounter = 0 # done self.initThresh() @@ -80,27 +100,67 @@ def __init__(self): # The functions that the subscribers call in order to get new data def updateMain(self, mainData: Float32) -> None: + """ + It is one of the functions that the subscribers call in order to get new data. + + Args: + mainData: it is a float value that is being passed in for a variable to be set to. + """ self.battery_voltage = mainData def update_FL(self, dataFL: Feedback) -> None: + """ + It is one of the functions that the subscribers call in order to get new data. + + Args: + dataFL: it is a feedback value that is being passed in for a variable to be set to. + """ self.voltageFL = dataFL.supply_voltage def update_FR(self, dataFR: Feedback) -> None: + """ + It is one of the functions that the subscribers call in order to get new data. + + Args: + dataFR: it is a feedback value that is being passed in for a variable to be set to. + """ self.voltageFR = dataFR.supply_voltage def update_BL(self, dataBL: Feedback) -> None: + """ + It is one of the functions that the subscribers call in order to get new data. + + Args: + dataBL: it is a feedback value that is being passed in for a variable to be set to. + """ self.voltageBL = dataBL.supply_voltage def update_BR(self, dataBR: Feedback) -> None: + """ + It is one of the functions that the subscribers call in order to get new data. + + Args: + dataBR: it is a feedback value that is being passed in for a variable to be set to. + """ self.voltageBR = dataBR.supply_voltage - # Part of signal that notifies program whenever window is resized - def resizeEvent(self, event): + def resizeEvent(self, event): # done + """ + Part of signal that notifies program whenever window is resized + + Args: + event: an event object is being passed in. + + Returns: + An event was being returned with a different size. + """ self.resized.emit() return super().resizeEvent(event) - # Increase/decrease size of fonts based on window resize - def resizeFont(self) -> None: + def resizeFont(self) -> None: # done + """ + Increase/decrease size of fonts based on window resize + """ # gets new window dimensions, the self is needed because we are referencing # our VoltageWidget class height = VoltageWidget.frameGeometry(self).height() @@ -124,8 +184,11 @@ def resizeFont(self) -> None: threshFont = QtGui.QFont("Times", (self.fontSize) / 3, QtGui.QFont.Bold) self.labelThresh.setFont(threshFont) - # Sets the text of the thrshold info box + # Sets the text of the threshold info box def initThresh(self) -> None: + """ + Sets the text of the threshold info box + """ # Low and Critical decide what colors the boxes take for # Good (Green), Warning (Yellow), and Critical (Red) # If the parameter server has not set these values then we use the DEFAULT @@ -159,8 +222,10 @@ def initThresh(self) -> None: threshFont = QtGui.QFont("Times", (self.fontSize) / 3, QtGui.QFont.Bold) self.labelThresh.setFont(threshFont) - # If self.gotParams is False, the updateLabel function calls testParams every 5 seconds - def testParams(self) -> None: + def testParams(self) -> None: # done + """ + If self.gotParams is False, the updateLabel function calls testParams every 5 seconds + """ try: self.lowThreshold = rospy.get_param("battery-voltage/low") self.criticalThreshold = rospy.get_param("battery-voltage/critical") @@ -180,8 +245,13 @@ def testParams(self) -> None: ) self.labelThresh.setText(threshText) - # sets colors of boxes based on current values of voltages for each box - def setColors(self, numMain: float) -> None: + def setColors(self, numMain: float) -> None: # done + """ + sets colors of boxes based on current values of voltages for each box + + Args: + numMain(float): box object that stores current values + """ if numMain > self.lowThreshold: self.labelMain.setStyleSheet( "QLabel { background-color : green; color : white; }", @@ -260,7 +330,9 @@ def setColors(self, numMain: float) -> None: ) def updateLabel(self) -> None: - # Tries self.gotParams every 3 function calls + """ + Tries self.gotParams every 3 function calls + """ self.paramCounter = self.paramCounter + 1 if self.gotParams is False and self.paramCounter >= 3: self.testParams() diff --git a/SubjuGator/command/subjugator_alarm/alarm_handlers/kill.py b/SubjuGator/command/subjugator_alarm/alarm_handlers/kill.py index d0d751f0a..4055694d8 100644 --- a/SubjuGator/command/subjugator_alarm/alarm_handlers/kill.py +++ b/SubjuGator/command/subjugator_alarm/alarm_handlers/kill.py @@ -101,10 +101,7 @@ def _bag_done_cb(self, status, result): rospy.loginfo(f"KILL BAG WRITTEN TO {result.filename}") else: rospy.logwarn( - "KILL BAG {}, status: {}".format( - TerminalState.to_string(status), - result.status, - ), + f"KILL BAG {TerminalState.to_string(status)}, status: {result.status}", ) def bagger_dump(self): @@ -117,7 +114,7 @@ def bagger_dump(self): rospy.logwarn("BAG_ALWAYS or BAG_KILL not set. Not making kill bag.") return goal = BagOnlineGoal(bag_name="kill.bag") - goal.topics = os.environ["BAG_ALWAYS"] + " " + os.environ["bag_kill"] + goal.topics = os.environ["BAG_ALWAYS"] + " " + os.environ["BAG_KILL"] self.bag_client.send_goal(goal, done_cb=self._bag_done_cb) def meta_predicate(self, meta_alarm, sub_alarms): @@ -170,5 +167,5 @@ def meta_predicate(self, meta_alarm, sub_alarms): # Raised if any alarms besides the two above are raised return any( - [alarm.raised for name, alarm in sub_alarms.items() if name not in ignore], + alarm.raised for name, alarm in sub_alarms.items() if name not in ignore ) diff --git a/SubjuGator/command/subjugator_alarm/alarm_handlers/odom_kill.py b/SubjuGator/command/subjugator_alarm/alarm_handlers/odom_kill.py index 4a7638909..90a8e817b 100644 --- a/SubjuGator/command/subjugator_alarm/alarm_handlers/odom_kill.py +++ b/SubjuGator/command/subjugator_alarm/alarm_handlers/odom_kill.py @@ -101,9 +101,7 @@ def check_continuity(self, new_odom_msg: Odometry): # True if 'continuous' if jump > self.MAX_JUMP: rospy.logerr("ODOM DISCONTINUITY DETECTED") self.ab.raise_alarm( - problem_description="ODOM DISCONTINUITY DETECTED JUMPED {} METERS".format( - jump, - ), + problem_description=f"ODOM DISCONTINUITY DETECTED JUMPED {jump} METERS", severity=5, ) self.odom_discontinuity = True @@ -120,9 +118,7 @@ def need_kill(self): if odom_loss: rospy.logerr_throttle( 1, - "LOST ODOM FOR {} SECONDS".format( - (rospy.Time.now() - self.last_time).to_sec(), - ), + f"LOST ODOM FOR {(rospy.Time.now() - self.last_time).to_sec()} SECONDS", ) self.ab.raise_alarm( problem_description="LOST ODOM FOR {} SECONDS".format( diff --git a/SubjuGator/command/subjugator_keyboard_control/teleop_twist_keyboard.py b/SubjuGator/command/subjugator_keyboard_control/teleop_twist_keyboard.py index ea5c7bedb..870df6ac7 100755 --- a/SubjuGator/command/subjugator_keyboard_control/teleop_twist_keyboard.py +++ b/SubjuGator/command/subjugator_keyboard_control/teleop_twist_keyboard.py @@ -141,9 +141,7 @@ def wait_for_subscribers(self): while not rospy.is_shutdown() and self.publisher.get_num_connections() == 0: if i == 4: print( - "Waiting for subscriber to connect to {}".format( - self.publisher.name, - ), + f"Waiting for subscriber to connect to {self.publisher.name}", ) rospy.sleep(0.5) i += 1 diff --git a/SubjuGator/command/subjugator_launch/config/adaptive_controller.yaml b/SubjuGator/command/subjugator_launch/config/adaptive_controller.yaml index c6406c44d..aefd3cb36 100644 --- a/SubjuGator/command/subjugator_launch/config/adaptive_controller.yaml +++ b/SubjuGator/command/subjugator_launch/config/adaptive_controller.yaml @@ -1,6 +1,6 @@ --- -kp: 1000, 1000, 1000, 5000, 5000, 5000 -kd: 150, 150, 150, 50, 100, 25 -kg: 5,5,5,5,5,5 -ki: 5,5,5,5,10,5 -use_learned: false +kp: 120, 150, 200, 100, 50, 50 +kd: 25, 25, 25, 25, 25, 25 +kg: 2.5,2.5,2.5,2.5,2.5,2.5 +ki: 2.5,2.5,2.5,2.5,2.5,2.5 +use_learned: true diff --git a/SubjuGator/command/subjugator_launch/config/adaptive_controller_gazebo.yaml b/SubjuGator/command/subjugator_launch/config/adaptive_controller_gazebo.yaml new file mode 100644 index 000000000..c6406c44d --- /dev/null +++ b/SubjuGator/command/subjugator_launch/config/adaptive_controller_gazebo.yaml @@ -0,0 +1,6 @@ +--- +kp: 1000, 1000, 1000, 5000, 5000, 5000 +kd: 150, 150, 150, 50, 100, 25 +kg: 5,5,5,5,5,5 +ki: 5,5,5,5,10,5 +use_learned: false diff --git a/SubjuGator/command/subjugator_launch/config/passive_sonar.yaml b/SubjuGator/command/subjugator_launch/config/passive_sonar.yaml index 9901f9703..e55aec3da 100644 --- a/SubjuGator/command/subjugator_launch/config/passive_sonar.yaml +++ b/SubjuGator/command/subjugator_launch/config/passive_sonar.yaml @@ -3,7 +3,7 @@ dist_h: 2.286e-02 # speed of sound in water v_sound: 1482 -# target Frquency in Hz +# target Frequency in Hz triggering/target_frequency: 30000 # tolerance around target frequerncy in Hz triggering/frequency_tolerance: 100 diff --git a/SubjuGator/command/subjugator_launch/launch/bag_debugging_controller.launch b/SubjuGator/command/subjugator_launch/launch/bag_debugging_controller.launch new file mode 100644 index 000000000..4c6024669 --- /dev/null +++ b/SubjuGator/command/subjugator_launch/launch/bag_debugging_controller.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/SubjuGator/command/subjugator_launch/launch/can.launch b/SubjuGator/command/subjugator_launch/launch/can.launch index 525671ace..b46fbd8ba 100644 --- a/SubjuGator/command/subjugator_launch/launch/can.launch +++ b/SubjuGator/command/subjugator_launch/launch/can.launch @@ -1,15 +1,13 @@ - + # Path of serial device - port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A800GHCF-if00-port0 + port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A50285BI-if00-port0 # Baudrate of device, should leave as is baudrate: 115200 - # The CAN device id of the usb-can board - can_id: 0 # List of python device handle classes device_handles: "18": sub8_thrust_and_kill_board.ThrusterAndKillBoard diff --git a/SubjuGator/command/subjugator_launch/launch/sub8.launch b/SubjuGator/command/subjugator_launch/launch/sub8.launch index 178efc6bb..247fe1e62 100644 --- a/SubjuGator/command/subjugator_launch/launch/sub8.launch +++ b/SubjuGator/command/subjugator_launch/launch/sub8.launch @@ -39,7 +39,9 @@ - + + + diff --git a/SubjuGator/command/subjugator_launch/launch/subsystems/adaptive_controller.launch b/SubjuGator/command/subjugator_launch/launch/subsystems/adaptive_controller.launch index 17bd1d3ac..a758c4278 100644 --- a/SubjuGator/command/subjugator_launch/launch/subsystems/adaptive_controller.launch +++ b/SubjuGator/command/subjugator_launch/launch/subsystems/adaptive_controller.launch @@ -1,8 +1,10 @@ - + + - - + + + diff --git a/SubjuGator/command/subjugator_launch/launch/subsystems/nav_box.launch b/SubjuGator/command/subjugator_launch/launch/subsystems/nav_box.launch index eb5e1bc31..fe18b292d 100644 --- a/SubjuGator/command/subjugator_launch/launch/subsystems/nav_box.launch +++ b/SubjuGator/command/subjugator_launch/launch/subsystems/nav_box.launch @@ -7,7 +7,7 @@ - + @@ -17,7 +17,7 @@ - + @@ -26,7 +26,7 @@ - + @@ -42,10 +42,10 @@ scale: - - [0.993770963897068, 0.00105871125374563, 7.659410525291767e-05] - - [0.00105871125374563, 0.9996814868251349, -0.0011040738267441828] - - [7.659410525291767e-05, -0.001104073826744163, 1.0065910531028952] - shift: [1.2551999807772446e-06, -1.1666595150804588e-06, 6.895773090438596e-08] + - [0.9991765357958566, 0.006242798579443988, -0.008472478269327878] + - [0.006242798579443957, 1.0016179705091928, 0.0059841151097914345] + - [-0.008472478269327836, 0.0059841151097913, 0.999354597532967] + shift: [7.889247409445414e-06, 4.879179471165382e-06, 7.46017199298374e-06] diff --git a/SubjuGator/command/subjugator_launch/package.xml b/SubjuGator/command/subjugator_launch/package.xml index 6a6beeeb6..e3f6ef4ba 100644 --- a/SubjuGator/command/subjugator_launch/package.xml +++ b/SubjuGator/command/subjugator_launch/package.xml @@ -5,8 +5,10 @@ The subjugator_launch package Jacob Panikulam MIT + rosbag roslaunch roslaunch + rosbag robot_state_publisher nodelet c3_trajectory_generator diff --git a/SubjuGator/command/subjugator_launch/scripts/depth_conn b/SubjuGator/command/subjugator_launch/scripts/depth_conn index 13bf8a480..c63922eb9 100755 --- a/SubjuGator/command/subjugator_launch/scripts/depth_conn +++ b/SubjuGator/command/subjugator_launch/scripts/depth_conn @@ -1,2 +1,2 @@ #!/bin/sh -exec socat -d -d pty,link=/tmp/depth,raw,echo=0 tcp:mil-sub-gumstix.ad.mil.ufl.edu:33056 +exec socat -d -d pty,link=/tmp/depth,raw,echo=0 tcp:192.168.37.61:33056 diff --git a/SubjuGator/command/subjugator_launch/scripts/dvl_conn b/SubjuGator/command/subjugator_launch/scripts/dvl_conn index f0be15464..745a45097 100755 --- a/SubjuGator/command/subjugator_launch/scripts/dvl_conn +++ b/SubjuGator/command/subjugator_launch/scripts/dvl_conn @@ -1,2 +1,2 @@ #!/bin/sh -exec socat -d -d pty,link=/tmp/dvl,raw,echo=0 tcp:mil-sub-gumstix.ad.mil.ufl.edu:349 +exec socat -d -d pty,link=/tmp/dvl,raw,echo=0 tcp:192.168.37.61:349 diff --git a/SubjuGator/command/subjugator_launch/scripts/imu_conn b/SubjuGator/command/subjugator_launch/scripts/imu_conn index 59275c393..bc93e1eb1 100755 --- a/SubjuGator/command/subjugator_launch/scripts/imu_conn +++ b/SubjuGator/command/subjugator_launch/scripts/imu_conn @@ -1,2 +1,2 @@ #!/bin/sh -exec socat -d -d pipe:/tmp/imu,wronly=1 tcp:mil-sub-gumstix.ad.mil.ufl.edu:1382 +exec socat -d -d pipe:/tmp/imu,wronly=1 tcp:192.168.37.61:1382 diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/arm_torpedos.py b/SubjuGator/command/subjugator_missions/subjugator_missions/arm_torpedos.py index aad130049..7a88e8908 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/arm_torpedos.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/arm_torpedos.py @@ -170,9 +170,7 @@ async def fire(self, target: str): ) self.print_good( - "{} locked. Firing torpedoes. Hit confirmed, good job Commander.".format( - target, - ), + f"{target} locked. Firing torpedoes. Hit confirmed, good job Commander.", ) sub_pos = await self.tx_pose() print("Current Sub Position: ", sub_pos) diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py b/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py index 6eafca064..ee2562491 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py @@ -617,8 +617,8 @@ def as_PoseTwist( def as_PoseTwistStamped( self, - linear: Sequence[int] = [0, 0, 0], - angular: Sequence[int] = [0, 0, 0], + linear: Sequence[float] = [0, 0, 0], + angular: Sequence[float] = [0, 0, 0], ) -> PoseTwistStamped: """ Returns a :class:`~mil_msgs.msg.PoseTwist` message class with the pose @@ -639,8 +639,8 @@ def as_PoseTwistStamped( def as_MoveToGoal( self, - linear: Sequence[int] = [0, 0, 0], - angular: Sequence[int] = [0, 0, 0], + linear: Sequence[float] = [0, 0, 0], + angular: Sequence[float] = [0, 0, 0], **kwargs, ) -> MoveToGoal: return MoveToGoal( diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/start_gate.py b/SubjuGator/command/subjugator_missions/subjugator_missions/start_gate.py index f395bd1e0..0886298e1 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/start_gate.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/start_gate.py @@ -160,9 +160,7 @@ def find_gate( p2 = rosmsg_to_numpy(o2.pose.position) if distance.euclidean(p, p2) > max_distance_away: fprint( - "Poles too far away. Distance {}".format( - distance.euclidean(p, p2), - ), + f"Poles too far away. Distance {distance.euclidean(p, p2)}", ) continue if distance.euclidean(p, p2) < min_distance_away: @@ -180,10 +178,7 @@ def find_gate( continue if abs(line[0]) < 1 and abs(line[1]) < 1: fprint( - "Objects on top of one another. x {}, y {}".format( - line[0], - line[1], - ), + f"Objects on top of one another. x {line[0]}, y {line[1]}", ) continue return (p, p2) diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/sub_singleton.py b/SubjuGator/command/subjugator_missions/subjugator_missions/sub_singleton.py index 693fc3bd7..8e498fc4d 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/sub_singleton.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/sub_singleton.py @@ -599,7 +599,7 @@ async def start_search_in_cone( distance_tol: float = 10, speed: float = 0.5, clear: bool = False, - c_func: Callable = None, + c_func: Callable | None = None, ): if clear: print("SONAR_OBJECTS: clearing pointcloud") diff --git a/SubjuGator/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/scripts/generate_config b/SubjuGator/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/scripts/generate_config index 5d12dad06..342292350 100755 --- a/SubjuGator/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/scripts/generate_config +++ b/SubjuGator/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/scripts/generate_config @@ -9,6 +9,7 @@ import rosbag import roslib import scipy.linalg import yaml +from mpl_toolkits.mplot3d import Axes3D from tf import transformations roslib.load_manifest("magnetic_hardsoft_compensation") @@ -20,7 +21,7 @@ def normalized_matrix(m): def calculate_error(points): - radii = map(numpy.linalg.norm, points) + radii = list(map(numpy.linalg.norm, points)) error = numpy.std(radii) / numpy.mean(radii) return error @@ -47,7 +48,7 @@ def fit_ellipsoid(points): B = numpy.ones((points.shape[0], 1)) - X = numpy.linalg.lstsq(A, B)[0].flatten() + X = numpy.linalg.lstsq(A, B, rcond=-1)[0].flatten() if X[0] < 0: X = -X # make sure ka turns out positive definite @@ -110,9 +111,7 @@ def test(): assert numpy.allclose( shift2, shift, - ), "Magnetic Hardsoft Compenstion self-test failed, shifts: {}".format( - (shift2, shift), - ) + ), f"Magnetic Hardsoft Compenstion self-test failed, shifts: {(shift2, shift)}" assert ( error < 1e-5 ), f"Magnetic Hardsoft Compenstion self-test failed, error: {error}" @@ -133,9 +132,11 @@ if __name__ == "__main__": with rosbag.Bag(sys.argv[1]) as bag: points = numpy.array( [ - mil_ros_tools.rosmsg_to_numpy(msg.magnetic_field) - if hasattr(msg, "magnetic_field") - else mil_ros_tools.rosmsg_to_numpy(msg.vector) + ( + mil_ros_tools.rosmsg_to_numpy(msg.magnetic_field) + if hasattr(msg, "magnetic_field") + else mil_ros_tools.rosmsg_to_numpy(msg.vector) + ) for topic, msg, t in bag.read_messages(topics=["/imu/mag_raw"]) ], ) @@ -152,7 +153,7 @@ if __name__ == "__main__": import matplotlib.pyplot as plt fig = plt.figure(figsize=(10, 10)) - ax = fig.add_subplot("111", projection="3d") + ax = Axes3D(fig) ax.scatter([0], [0], [0], s=100, c="r") ax.scatter(*zip(*points[::10, :])) axisEqual3D(ax) @@ -170,7 +171,7 @@ if __name__ == "__main__": import matplotlib.pyplot as plt fig = plt.figure(figsize=(10, 10)) - ax = fig.add_subplot("111", projection="3d") + ax = Axes3D(fig) ax.scatter([0], [0], [0], s=100, c="r") ax.scatter(*zip(*points[::10, :])) ax.scatter(*zip(*compensated[::10, :]), c="g") @@ -185,8 +186,8 @@ if __name__ == "__main__": print( yaml.dump( { - "scale": (map(float, x) for x in scale), - "shift": map(float, shift), + "scale": scale.tolist(), + "shift": shift.tolist(), }, ), ) diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py index 9db8c34be..cee7efb30 100644 --- a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py +++ b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 import rospy -from mil_usb_to_can import CANDeviceHandle +from mil_usb_to_can.sub8 import CANDeviceHandle from sub8_actuator_board.srv import SetValve, SetValveRequest diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py index 73536f7e5..f287e09fa 100644 --- a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py +++ b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py @@ -1,6 +1,6 @@ import struct -from mil_usb_to_can import ApplicationPacket +from mil_usb_to_can.sub8 import ApplicationPacket # CAN ID for the channel from MOBO to actuator board SEND_ID = 0x51 diff --git a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py index af0867fce..f776274d2 100644 --- a/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py +++ b/SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py @@ -1,5 +1,5 @@ #!/usr/bin/python3 -from mil_usb_to_can import SimulatedCANDevice +from mil_usb_to_can.sub8 import SimulatedCANDevice from .packets import SEND_ID, CommandMessage, FeedbackMessage diff --git a/SubjuGator/drivers/sub8_adis16400_imu/include/adis16400_imu/driver.h b/SubjuGator/drivers/sub8_adis16400_imu/include/adis16400_imu/driver.h index b62785f36..dc5146e7f 100644 --- a/SubjuGator/drivers/sub8_adis16400_imu/include/adis16400_imu/driver.h +++ b/SubjuGator/drivers/sub8_adis16400_imu/include/adis16400_imu/driver.h @@ -41,7 +41,7 @@ class Device } catch (const std::exception &exc) { - ROS_ERROR("error on open(%s): %s; reopening after delay", port.c_str(), exc.what()); + ROS_ERROR_THROTTLE(5, "error on open(%s): %s; reopening after delay", port.c_str(), exc.what()); boost::this_thread::sleep(boost::posix_time::seconds(1)); return false; } @@ -62,7 +62,7 @@ class Device } catch (const std::exception &exc) { - ROS_ERROR("error on read: %s; reopening", exc.what()); + ROS_ERROR_THROTTLE(5, "error on read: %s; reopening", exc.what()); open(); return false; } @@ -70,7 +70,7 @@ class Device result.header.frame_id = frame_id; result.header.stamp = ros::Time::now(); - result.orientation_covariance[0] = -1; // indicate no orientation data + result.orientation_covariance[0] = -1; // indicate no orientation data static const double GYRO_CONVERSION = 0.05 * (2 * M_PI / 360); // convert to deg/s and then to rad/s result.angular_velocity.x = get16(data + 4 + 2 * 0) * GYRO_CONVERSION; @@ -78,7 +78,7 @@ class Device result.angular_velocity.z = get16(data + 4 + 2 * 2) * GYRO_CONVERSION; result.angular_velocity_covariance[0] = result.angular_velocity_covariance[4] = result.angular_velocity_covariance[8] = - pow(0.9 * (2 * M_PI / 360), 2); // 0.9 deg/sec rms converted to rad/sec and then squared + pow(0.9 * (2 * M_PI / 360), 2); // 0.9 deg/sec rms converted to rad/sec and then squared static const double ACC_CONVERSION = 3.33e-3 * 9.80665; // convert to g's and then to m/s^2 result.linear_acceleration.x = -get16(data + 10 + 2 * 0) * ACC_CONVERSION; @@ -99,9 +99,9 @@ class Device mag_result.magnetic_field_covariance[8] = pow(1.25e-3 * 0.0001, 2); // 1.25 mgauss converted to tesla and then squared - getu16(data + 0); // flags unused - getu16(data + 2) * 2.418e-3; // supply voltage unused - get16(data + 22) * 0.14 + 25; // temperature unused + getu16(data + 0); // flags unused + getu16(data + 2) * 2.418e-3; // supply voltage unused + get16(data + 22) * 0.14 + 25; // temperature unused return true; } diff --git a/SubjuGator/drivers/sub8_depth_driver/include/depth_driver/driver.h b/SubjuGator/drivers/sub8_depth_driver/include/depth_driver/driver.h index e9c287107..2f4468091 100644 --- a/SubjuGator/drivers/sub8_depth_driver/include/depth_driver/driver.h +++ b/SubjuGator/drivers/sub8_depth_driver/include/depth_driver/driver.h @@ -69,7 +69,7 @@ class Device } catch (const std::exception &exc) { - ROS_ERROR("error on write: %s; dropping", exc.what()); + ROS_ERROR_THROTTLE(5, "error on write: %s; dropping", exc.what()); } } @@ -82,7 +82,7 @@ class Device } catch (const std::exception &exc) { - ROS_ERROR("error on read: %s; reopening", exc.what()); + ROS_ERROR_THROTTLE(5, "error on read: %s; reopening", exc.what()); open(); return false; } @@ -98,7 +98,7 @@ class Device } catch (const std::exception &exc) { - ROS_ERROR("error on open(%s): %s; reopening after delay", port.c_str(), exc.what()); + ROS_ERROR_THROTTLE(5, "error on open(%s): %s; reopening after delay", port.c_str(), exc.what()); boost::this_thread::sleep(boost::posix_time::seconds(1)); } } @@ -166,7 +166,7 @@ class Device void send_heartbeat() { - send_packet(ByteVec()); // heartbeat + send_packet(ByteVec()); // heartbeat uint8_t msg[] = { 4, 1, 20 }; send_packet(ByteVec(msg, msg + sizeof(msg) / sizeof(msg[0]))); // StartPublishing 20hz } diff --git a/SubjuGator/drivers/sub8_rdi_dvl/include/rdi_explorer_dvl/driver.hpp b/SubjuGator/drivers/sub8_rdi_dvl/include/rdi_explorer_dvl/driver.hpp index 914706c44..80e9cf973 100644 --- a/SubjuGator/drivers/sub8_rdi_dvl/include/rdi_explorer_dvl/driver.hpp +++ b/SubjuGator/drivers/sub8_rdi_dvl/include/rdi_explorer_dvl/driver.hpp @@ -48,7 +48,7 @@ class Device } catch (const std::exception &exc) { - ROS_ERROR_THROTTLE(0.5, "DVL: error on read: %s; reopening serial port", exc.what()); + ROS_ERROR_THROTTLE(5, "DVL: error on read: %s; reopening serial port", exc.what()); open(); return false; } @@ -86,7 +86,7 @@ class Device } catch (const std::exception &exc) { - ROS_ERROR("DVL: error on open(port=%s): %s; reopening after delay", port.c_str(), exc.what()); + ROS_ERROR_THROTTLE(5, "DVL: error on open(port=%s): %s; reopening after delay", port.c_str(), exc.what()); boost::this_thread::sleep(boost::posix_time::seconds(1)); } } @@ -268,7 +268,7 @@ class Device } catch (const std::exception &exc) { - ROS_ERROR_THROTTLE(0.5, "DVL: error on write: %s; dropping heartbeat", exc.what()); + ROS_ERROR_THROTTLE(5, "DVL: error on write: %s; dropping heartbeat", exc.what()); } } diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py index 336e0336d..19df02c33 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py @@ -1,6 +1,6 @@ #!/usr/bin/python3 import rospy -from mil_usb_to_can import CANDeviceHandle +from mil_usb_to_can.sub8 import CANDeviceHandle from ros_alarms import AlarmBroadcaster, AlarmListener from ros_alarms_msgs.msg import Alarm from rospy.timer import TimerEvent @@ -26,7 +26,7 @@ class ThrusterAndKillBoard(CANDeviceHandle): def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) # Initialize thruster mapping from params - self.thrusters = make_thruster_dictionary( + self.thrusters, self.name_to_id = make_thruster_dictionary( rospy.get_param("/thruster_layout/thrusters"), ) # Tracks last hw-kill alarm update @@ -93,7 +93,7 @@ def on_hw_kill(self, alarm: Alarm) -> None: Update the classes' hw-kill alarm to the latest update. Args: - alarm (:class:`~ros_alarms.msg._Alarm.Alarm`): The alarm message to update with. + alarm (:class:`~ros_alarms_msgs.msg._Alarm.Alarm`): The alarm message to update with. """ self._last_hw_kill = alarm @@ -109,16 +109,14 @@ def on_command(self, msg: Thrust) -> None: # If we don't have a mapping for this thruster, ignore it if cmd.name not in self.thrusters: rospy.logwarn( - "Command received for {}, but this is not a thruster.".format( - cmd.name, - ), + f"Command received for {cmd.name}, but this is not a thruster.", ) continue # Map commanded thrust (in newetons) to effort value (-1 to 1) effort = self.thrusters[cmd.name].effort_from_thrust(cmd.thrust) # Send packet to command specified thruster the specified force packet = ThrustPacket.create_thrust_packet( - ThrustPacket.ID_MAPPING[cmd.name], + self.name_to_id[cmd.name], effort, ) self.send_data(bytes(packet), can_id=THRUST_SEND_ID) diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py index 519d4e829..dcd1aaa0b 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py @@ -1,7 +1,7 @@ import struct from collections import namedtuple -from mil_usb_to_can import ApplicationPacket +from mil_usb_to_can.sub8 import ApplicationPacket # CAN channel to send thrust messages to THRUST_SEND_ID = 0x21 @@ -250,41 +250,9 @@ class ThrustPacket(ApplicationPacket): Attributes: IDENTIFIER (int): The packet identifier, equal to the ordinal value of "T," or 84. - ID_MAPPING (Dict[str, int]): A dictionary mapping 3-letter thruster codes - to their respective IDs: - - +--------+------+ - | Name | ID | - +========+======+ - | FLH | 0 | - +--------+------+ - | FRH | 1 | - +--------+------+ - | FLV | 2 | - +--------+------+ - | FRV | 3 | - +--------+------+ - | BLH | 4 | - +--------+------+ - | BRH | 5 | - +--------+------+ - | BLV | 6 | - +--------+------+ - | BRV | 7 | - +--------+------+ """ IDENTIFIER = ord("T") - ID_MAPPING = { - "FLH": 0, - "FRH": 1, - "FLV": 2, - "FRV": 3, - "BLH": 4, - "BRH": 5, - "BLV": 6, - "BRV": 7, - } @classmethod def create_thrust_packet(cls, thruster_id: int, command: float): @@ -319,7 +287,4 @@ def command(self) -> float: return struct.unpack("f", self.payload[1:])[0] def __str__(self): - return "ThrustPacket(thruster_id={}, command={})".format( - self.thruster_id, - self.command, - ) + return f"ThrustPacket(thruster_id={self.thruster_id}, command={self.command})" diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py index a24a4ab95..83c595815 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py @@ -1,6 +1,6 @@ #!/usr/bin/python3 import rospy -from mil_usb_to_can import SimulatedCANDevice +from mil_usb_to_can.sub8 import SimulatedCANDevice from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse from .packets import ( @@ -156,4 +156,4 @@ def on_data(self, data: bytes, can_id: int) -> None: packet = HeartbeatMessage.from_bytes(data) self._last_heartbeat = rospy.Time.now() else: - assert False, "No recognized identifier" + raise Exception("No recognized identifier") diff --git a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/thruster.py b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/thruster.py index 5a90baac0..7fe9aef12 100644 --- a/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/thruster.py +++ b/SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/thruster.py @@ -1,16 +1,21 @@ -from typing import Any, Dict +from __future__ import annotations + +from typing import Any from numpy import clip, polyval -def make_thruster_dictionary(dictionary): +def make_thruster_dictionary(dictionary) -> tuple[dict[str, Thruster], dict[str, int]]: """ - Make a dictionary mapping thruster names to :class:`Thruster` objects. + Make a dictionary mapping thruster names to :class:`Thruster` objects + and a dictionary mapping thruster names to node IDs. """ ret = {} + name_id_map = {} for thruster, content in dictionary.items(): ret[thruster] = Thruster.from_dict(content) - return ret + name_id_map[thruster] = content["node_id"] + return ret, name_id_map class Thruster: @@ -27,7 +32,7 @@ def __init__(self, forward_calibration, backward_calibration): self.backward_calibration = backward_calibration @classmethod - def from_dict(cls, data: Dict[str, Dict[str, Any]]): + def from_dict(cls, data: dict[str, dict[str, Any]]): """ Constructs the class from a dictionary. The dictionary should be formatted as so: diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/CMakeLists.txt b/SubjuGator/drivers/sub9_thrust_and_kill_board/CMakeLists.txt new file mode 100644 index 000000000..19b63afff --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 2.8.3) +project(sub9_thrust_and_kill_board) +find_package(catkin REQUIRED COMPONENTS + mil_usb_to_can +) +add_rostest(test/simulated_board.test) +catkin_python_setup() +catkin_package() diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/package.xml b/SubjuGator/drivers/sub9_thrust_and_kill_board/package.xml new file mode 100644 index 000000000..530dcfdf9 --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/package.xml @@ -0,0 +1,13 @@ + + + sub9_thrust_and_kill_board + 0.0.0 + The sub9_thrust_and_kill_board package + Cameron Brown + MIT + Cameron Brown + catkin + mil_usb_to_can + mil_usb_to_can + mil_usb_to_can + diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/setup.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/setup.py new file mode 100644 index 000000000..32234daad --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/setup.py @@ -0,0 +1,11 @@ +# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import setup + +# Fetch values from package.xml +setup_args = generate_distutils_setup( + packages=["sub9_thrust_and_kill_board"], +) + +setup(**setup_args) diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/__init__.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/__init__.py new file mode 100644 index 000000000..7a9f7f666 --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/__init__.py @@ -0,0 +1,11 @@ +from .handle import ThrusterAndKillBoard +from .packets import ( + HeartbeatReceivePacket, + HeartbeatSetPacket, + KillReceivePacket, + KillSetPacket, + KillStatus, + ThrustSetPacket, +) +from .simulation import ThrusterAndKillBoardSimulation +from .thruster import Thruster diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py new file mode 100644 index 000000000..8d103cc78 --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/handle.py @@ -0,0 +1,196 @@ +#!/usr/bin/python3 +from __future__ import annotations + +import rospy +from mil_misc_tools import rospy_to_datetime +from mil_usb_to_can.sub9 import AckPacket, CANDeviceHandle, NackPacket +from ros_alarms import AlarmBroadcaster, AlarmListener +from ros_alarms_msgs.msg import Alarm +from rospy.timer import TimerEvent +from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse +from subjugator_msgs.msg import Thrust + +from .packets import ( + HeartbeatReceivePacket, + HeartbeatSetPacket, + KillReceivePacket, + KillSetPacket, + KillStatus, + ThrustSetPacket, +) +from .thruster import make_thruster_dictionary + + +class ThrusterAndKillBoard(CANDeviceHandle): + """ + Device handle for the thrust and kill board. + """ + + ID_MAPPING = { + "FLH": 0, + "FRH": 1, + "FLV": 2, + "FRV": 3, + "BLH": 4, + "BRH": 5, + "BLV": 6, + "BRV": 7, + } + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + # Initialize thruster mapping from params + self.thrusters = make_thruster_dictionary( + rospy.get_param("/thruster_layout/thrusters"), + ) + # Tracks last hw-kill alarm update + self._last_hw_kill = None + # Used to raise/clear hw-kill when board updates + self._kill_broadcaster = AlarmBroadcaster("hw-kill") + # Listens to hw-kill updates to ensure another nodes doesn't manipulate it + self._hw_kill_listener = AlarmListener( + "hw-kill", + callback_funct=self.on_hw_kill, + ) + # Provide service for alarm handler to set/clear the motherboard kill + self._unkill_service = rospy.Service("/set_mobo_kill", SetBool, self.set_kill) + # Sends heartbeat to board + self._heartbeat_timer = rospy.Timer(rospy.Duration(0.4), self.send_heartbeat) + # Create a subscribe for thruster commands + self._sub = rospy.Subscriber( + "/thrusters/thrust", + Thrust, + self.on_command, + queue_size=10, + ) + self._last_heartbeat = rospy.Time.now() + self._last_packet = None + self._updated_kill = False + + def set_kill(self, req: SetBoolRequest) -> SetBoolResponse: + """ + Called on service calls to ``/set_mobo_kill``, sending the appropriate + packet to the board to unassert or assert to motherboard-origin kill. + + Args: + req (SetBoolRequest): The service request. + + Returns: + SetBoolResponse: The service response. + """ + self.send_data(KillSetPacket(req.data, KillStatus.SOFTWARE_REQUESTED)) + start = rospy.Time.now() + while rospy.Time.now() - start < rospy.Duration(1): + if self._last_packet is not None: + break + + if isinstance(self._last_packet, NackPacket): + raise RuntimeError("Request not acknowledged.") + + start = rospy.Time.now() + while rospy.Time.now() - start < rospy.Duration(1): + if self._updated_kill: + break + + if self._updated_kill: + self._updated_kill = False + return SetBoolResponse(success=True) + else: + return SetBoolResponse( + success=False, + message="No response from board after 1 second.", + ) + + def send_heartbeat(self, _: TimerEvent) -> None: + """ + Send a special heartbeat packet. Called by a recurring timer set upon + initialization. + """ + self.send_data(HeartbeatSetPacket()) + + def on_hw_kill(self, alarm: Alarm) -> None: + """ + Update the classes' hw-kill alarm to the latest update. + + Args: + alarm (:class:`~ros_alarms_msgs.msg._Alarm.Alarm`): The alarm message to update with. + """ + self._last_hw_kill = alarm + + def on_command(self, msg: Thrust) -> None: + """ + When a thrust command message is received from the Subscriber, send the appropriate packet + to the board. + + Args: + msg (Thrust): The thrust message. + """ + for cmd in msg.thruster_commands: + # If we don't have a mapping for this thruster, ignore it + if cmd.name not in self.thrusters: + rospy.logwarn( + f"Command received for {cmd.name}, but this is not a thruster.", + ) + continue + # Map commanded thrust (in newetons) to effort value (-1 to 1) + effort = self.thrusters[cmd.name].effort_from_thrust(cmd.thrust) + # Send packet to command specified thruster the specified force + packet = ThrustSetPacket(self.ID_MAPPING[cmd.name], effort) + self.send_data(packet) + + def update_software_kill(self, raised: bool, message: str): + # If the current status differs from the alarm, update the alarm + severity = 2 if raised else 0 + self._updated_kill = True + self._hw_kill_listener.wait_for_server() + if ( + self._last_hw_kill is None + or self._last_hw_kill.raised != raised + or self._last_hw_kill.problem_description != message + or self._last_hw_kill.severity != severity + ): + if raised: + self._kill_broadcaster.raise_alarm( + severity=severity, + problem_description=message, + ) + else: + self._kill_broadcaster.clear_alarm(severity=severity) + + def on_data( + self, + data: AckPacket | NackPacket | HeartbeatReceivePacket | KillReceivePacket, + ) -> None: + """ + Parse the two bytes and raise kills according to a set of specifications + listed below. + """ + if isinstance(data, (AckPacket, NackPacket)): + self._last_packet = data + elif isinstance(data, HeartbeatReceivePacket): + self._last_heartbeat = rospy.Time.now() + elif isinstance(data, KillReceivePacket): + if data.set is False: + self.update_software_kill(False, "") + elif data.status is KillStatus.MOBO_HEARTBEAT_LOST: + self.update_software_kill( + True, + "Thrust/kill board lost heartbeat from motherboard.", + ) + elif data.status is KillStatus.BATTERY_LOW: + self.update_software_kill(True, "Battery too low.") + elif data.status is KillStatus.KILL_SWITCH: + self.update_software_kill(True, "Kill switch was pulled!") + elif data.status is KillStatus.BOARD_HEARTBEAT_LOST: + dt = rospy_to_datetime(self._last_heartbeat) + self.update_software_kill( + True, + f"Motherboard is no longer hearing heartbeat from thrust/kill board. Last heard from board at {dt}.", + ) + elif data.status is KillStatus.SOFTWARE_REQUESTED: + self.update_software_kill( + True, + "Software requested kill.", + ) + else: + raise ValueError(f"Not expecting packet of type {data.__class__.__name__}!") diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py new file mode 100644 index 000000000..93d892fd3 --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py @@ -0,0 +1,98 @@ +from dataclasses import dataclass +from enum import IntEnum + +from mil_usb_to_can.sub9 import Packet + + +@dataclass +class HeartbeatSetPacket(Packet, msg_id=0x02, subclass_id=0x00, payload_format=""): + """ + Heartbeat packet sent by the motherboard to the thrust/kill board. + """ + + +@dataclass +class HeartbeatReceivePacket(Packet, msg_id=0x02, subclass_id=0x01, payload_format=""): + """ + Heartbeat packet sent by the thrust/kill board to the motherboard. + """ + + +@dataclass +class ThrustSetPacket(Packet, msg_id=0x02, subclass_id=0x02, payload_format="=Bf"): + """ + Packet to set the speed of a specific thruster. + + Attributes: + thruster_id (int): The ID of the thruster to set the speed of. The ID of + the thruster corresponds to a specific thruster: + + +--------+------+ + | name | id | + +========+======+ + | FLH | 0 | + +--------+------+ + | FRH | 1 | + +--------+------+ + | FLV | 2 | + +--------+------+ + | FRV | 3 | + +--------+------+ + | BLH | 4 | + +--------+------+ + | BRH | 5 | + +--------+------+ + | BLV | 6 | + +--------+------+ + | BRV | 7 | + +--------+------+ + speed (float): The speed to set the thruster to. + """ + + thruster_id: int + speed: float + + +class KillStatus(IntEnum): + """ + Enum to represent the purpose behind a kill. + """ + + #: A software user manually requested a kill on the sub. + SOFTWARE_REQUESTED = 0 + #: The board reported that it stopped hearing the heartbeat from the motherboard. + MOBO_HEARTBEAT_LOST = 1 + #: The motherboard reported that it stopped hearing the heartbeat from the board. + BOARD_HEARTBEAT_LOST = 2 + #: The physical kill switch was pulled, requesting an immediate kill. + KILL_SWITCH = 3 + #: The battery is too low to continue operation. + BATTERY_LOW = 4 + + +@dataclass +class KillSetPacket(Packet, msg_id=0x02, subclass_id=0x03, payload_format="=BB"): + """ + Packet sent by the motherboard to set/unset the kill. + + Attributes: + set (bool): Whether the kill is set or unset. + status (KillStatus): The reason for the kill. + """ + + set: bool + status: KillStatus + + +@dataclass +class KillReceivePacket(Packet, msg_id=0x02, subclass_id=0x04, payload_format="=BB"): + """ + Packet sent by the motherboard to set/unset the kill. + + Attributes: + set (bool): Whether the kill is set or unset. + status (KillStatus): The reason for the kill. + """ + + set: bool + status: KillStatus diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/simulation.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/simulation.py new file mode 100644 index 000000000..52deb3181 --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/simulation.py @@ -0,0 +1,92 @@ +#!/usr/bin/python3 +from __future__ import annotations + +import rospy +from mil_usb_to_can.sub9 import AckPacket, NackPacket, SimulatedCANDeviceHandle +from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse + +from .packets import ( + HeartbeatReceivePacket, + HeartbeatSetPacket, + KillReceivePacket, + KillSetPacket, + KillStatus, + ThrustSetPacket, +) + + +class ThrusterAndKillBoardSimulation(SimulatedCANDeviceHandle): + """ + Serial simulator for the thruster and kill board, + providing services to simulate physical plug connections/disconnections. + + Inherits from :class:`~mil_usb_to_can.SimulatedCANDevice`. + + Attributes: + kill (bool): Whether the hard kill was set. + """ + + HEARTBEAT_TIMEOUT_SECONDS = rospy.Duration(1.0) + + def __init__(self, *args, **kwargs): + self.kill = False + self._last_heartbeat = None + super().__init__(*args, **kwargs) + self._update_timer = rospy.Timer(rospy.Duration(1), self._check_for_timeout) + self._kill_srv = rospy.Service("/simulate_kill", SetBool, self.set_kill) + + def _check_for_timeout(self, _: rospy.timer.TimerEvent): + if self.heartbeat_timedout and not self.kill: + self.send_data(bytes(KillSetPacket(True, KillStatus.BOARD_HEARTBEAT_LOST))) + self.kill = True + + def set_kill(self, req: SetBoolRequest) -> SetBoolResponse: + """ + Called by the `/simulate_kill` service to set the kill state of the + simluated device. + + Args: + req (SetBoolRequest): The request to set the service with. + + Returns: + SetBoolResponse: The response to the service that the operation was successful. + """ + self.kill = req.data + return SetBoolResponse(success=True) + + @property + def heartbeat_timedout(self) -> bool: + """ + Whether the heartbeat timed out. + + Returns: + bool: The status of the heartbeat timing out. + """ + return ( + self._last_heartbeat is None + or (rospy.Time.now() - self._last_heartbeat) + > self.HEARTBEAT_TIMEOUT_SECONDS + ) + + def on_data( + self, + packet: HeartbeatSetPacket | ThrustSetPacket | KillSetPacket, + ) -> None: + """ + Serves as the data handler for the device. Handles :class:`KillMessage`, + :class:`ThrustPacket`, and :class:`HeartbeatMessage` types. + """ + if isinstance(packet, HeartbeatSetPacket): + self._last_heartbeat = rospy.Time.now() + self.send_data(bytes(HeartbeatReceivePacket())) + + elif isinstance(packet, ThrustSetPacket): + self.send_data(bytes(AckPacket())) + + elif isinstance(packet, KillSetPacket): + self.kill = packet.set + self.send_data(bytes(AckPacket())) + self.send_data(bytes(KillReceivePacket(packet.set, packet.status))) + + else: + self.send_data(bytes(NackPacket())) diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/thruster.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/thruster.py new file mode 100644 index 000000000..5a90baac0 --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/thruster.py @@ -0,0 +1,66 @@ +from typing import Any, Dict + +from numpy import clip, polyval + + +def make_thruster_dictionary(dictionary): + """ + Make a dictionary mapping thruster names to :class:`Thruster` objects. + """ + ret = {} + for thruster, content in dictionary.items(): + ret[thruster] = Thruster.from_dict(content) + return ret + + +class Thruster: + """ + Models the force (thrust) to PWM (effort) of a thruster. + + Attributes: + forward_calibration (???): ??? + backward_calibration (???): ??? + """ + + def __init__(self, forward_calibration, backward_calibration): + self.forward_calibration = forward_calibration + self.backward_calibration = backward_calibration + + @classmethod + def from_dict(cls, data: Dict[str, Dict[str, Any]]): + """ + Constructs the class from a dictionary. The dictionary should be formatted + as so: + + .. code-block:: python3 + + { + "calib": { + "forward": ..., + "backward": ..., + } + } + + Args: + data (Dict[str, Dict[str, Any]]): The dictionary containing the initialization info. + """ + forward_calibration = data["calib"]["forward"] + backward_calibration = data["calib"]["backward"] + return cls(forward_calibration, backward_calibration) + + def effort_from_thrust_unclipped(self, thrust: Any): + if thrust < 0: + return polyval(self.backward_calibration, thrust) + else: + return polyval(self.forward_calibration, thrust) + + def effort_from_thrust(self, thrust: Any): + """ + Attempts to find the effort from a particular value of thrust. + + Args: + thrust (???): The amount of thrust. + """ + unclipped = self.effort_from_thrust_unclipped(thrust) + # Theoretically can limit to .66 under 16V assumptions or .5 under 12V assumptions... So do both (.5 + 66)/2 + return clip(unclipped, -0.58, 0.58) diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board.test b/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board.test new file mode 100644 index 000000000..44904f746 --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board.test @@ -0,0 +1,18 @@ + + + + + + + device_handles: + sub9_thrust_and_kill_board.ThrusterAndKillBoard: [sub9_thrust_and_kill_board.KillReceivePacket] + simulated_devices: + sub9_thrust_and_kill_board.ThrusterAndKillBoardSimulation: [sub9_thrust_and_kill_board.HeartbeatSetPacket, sub9_thrust_and_kill_board.ThrustSetPacket, sub9_thrust_and_kill_board.KillSetPacket] + + + + + + + + diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py new file mode 100755 index 000000000..4af9813d7 --- /dev/null +++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python3 +import unittest + +import rospy +from ros_alarms import AlarmListener +from std_srvs.srv import SetBool, SetBoolRequest + + +class SimulatedBoardTest(unittest.TestCase): + """ + Integration test for CAN2USB board driver. Talks + to a simulated CAN device which should add two integers + """ + + def __init__(self, *args): + self.kill_srv = rospy.ServiceProxy("/set_mobo_kill", SetBool) + self.hw_alarm_listener = AlarmListener("hw-kill") + super().__init__(*args) + + def test_correct_response(self): + """ + Test we can get correct data through CAN bus at least ten times. + """ + self.kill_srv.wait_for_service(5) + self.hw_alarm_listener.wait_for_server() + self.assertTrue(self.kill_srv(SetBoolRequest(True)).success) + self.assertTrue(self.hw_alarm_listener.is_raised(True)) + self.assertTrue(self.kill_srv(SetBoolRequest(False)).success) + self.assertTrue(self.hw_alarm_listener.is_raised(False)) + + +if __name__ == "__main__": + rospy.init_node("simulated_board_test", anonymous=True) + import rostest + + rostest.rosrun( + "sub9_thrust_and_kill_board", + "simulated_board_test", + SimulatedBoardTest, + ) + unittest.main() diff --git a/SubjuGator/drivers/sub_actuator_board/CMakeLists.txt b/SubjuGator/drivers/sub_actuator_board/CMakeLists.txt new file mode 100644 index 000000000..9309bb083 --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.0.2) +project(sub_actuator_board) +find_package(catkin REQUIRED COMPONENTS + message_generation + mil_usb_to_can +) +catkin_python_setup() +add_service_files( + FILES + SetValve.srv + GetValve.srv +) +add_rostest(test/simulated_board.test) +generate_messages() +catkin_package(CATKIN_DEPENDS message_runtime) diff --git a/SubjuGator/drivers/sub_actuator_board/package.xml b/SubjuGator/drivers/sub_actuator_board/package.xml new file mode 100644 index 000000000..0ab794680 --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/package.xml @@ -0,0 +1,15 @@ + + + sub_actuator_board + 0.0.0 + The sub_actuator_board package + Kevin Allen + MIT + Kevin Allen + catkin + message_generation + mil_usb_to_can + mil_usb_to_can + message_runtime + mil_usb_to_can + diff --git a/SubjuGator/drivers/sub_actuator_board/setup.py b/SubjuGator/drivers/sub_actuator_board/setup.py new file mode 100644 index 000000000..42e9f57e8 --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/setup.py @@ -0,0 +1,9 @@ +# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import setup + +# Fetch values from package.xml +setup_args = generate_distutils_setup(packages=["sub_actuator_board"]) + +setup(**setup_args) diff --git a/SubjuGator/drivers/sub_actuator_board/srv/GetValve.srv b/SubjuGator/drivers/sub_actuator_board/srv/GetValve.srv new file mode 100644 index 000000000..dff0ce39b --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/srv/GetValve.srv @@ -0,0 +1,4 @@ +# Service call to get the status of a pneumatic valve +uint8 actuator # ID of valve +--- +bool opened # Whether the valve has been opened diff --git a/SubjuGator/drivers/sub_actuator_board/srv/SetValve.srv b/SubjuGator/drivers/sub_actuator_board/srv/SetValve.srv new file mode 100644 index 000000000..837be5cd7 --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/srv/SetValve.srv @@ -0,0 +1,6 @@ +# Service call to open or close a pneumatic valve +uint8 actuator # ID of valve +bool opened # If True, valve will open; if False, valve will close +--- +bool success # False if there were any problems with the request. +string message # Description of error if success is false diff --git a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/__init__.py b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/__init__.py new file mode 100644 index 000000000..8d260d68e --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/__init__.py @@ -0,0 +1,7 @@ +from .handle import ActuatorBoard +from .packets import ( + ActuatorPollRequestPacket, + ActuatorPollResponsePacket, + ActuatorSetPacket, +) +from .simulation import ActuatorBoardSimulation diff --git a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/handle.py b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/handle.py new file mode 100644 index 000000000..0da860670 --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/handle.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import rospy +from mil_usb_to_can.sub9 import AckPacket, CANDeviceHandle + +from sub_actuator_board.srv import ( + GetValve, + GetValveRequest, + GetValveResponse, + SetValve, + SetValveRequest, +) + +from .packets import ( + ActuatorPollRequestPacket, + ActuatorPollResponsePacket, + ActuatorSetPacket, +) + + +class ActuatorBoard(CANDeviceHandle): + """ + Device handle for the actuator board. Because this class implements a CAN device, + it inherits from the :class:`CANDeviceHandle` class. + """ + + _recent_response: ActuatorPollResponsePacket | None + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self._set_service = rospy.Service("/set_valve", SetValve, self.set_valve) + self._get_service = rospy.Service("/get_valve", GetValve, self.get_valve) + self._recent_response = None + + def set_valve(self, req: SetValveRequest) -> dict: + """ + Called when the ``/set_valve`` service is requested. Creates a message to + control the valve and sends it through the inherited device handle. + + Args: + req (SetValveRequest): Request to set the valve. + + Returns: + dict: List of information which is casted into a SetValveResponse. + """ + # Send board command to open or close specified valve + message = ActuatorSetPacket(address=req.actuator, open=req.opened) + self.send_data(message) + rospy.loginfo( + "Set valve {} {}".format( + req.actuator, + "opened" if req.opened else "closed", + ), + ) + # Wait some time for board to process command + rospy.sleep(0.01) + # Request the status of the valve just commanded to ensure it worked + self.send_data(ActuatorPollRequestPacket()) + start = rospy.Time.now() + while rospy.Time.now() - start < rospy.Duration(2): + if self._recent_response is not None: + break + + success = False + if self._recent_response is not None: + if not req.opened: + success = not (self._recent_response.values & (1 << req.actuator)) + else: + success = self._recent_response.values & (1 << req.actuator) + response = {"success": success} + self._recent_response = None + return response + + def get_valve(self, req: GetValveRequest) -> GetValveResponse: + message = ActuatorPollRequestPacket() + self.send_data(message) + start = rospy.Time.now() + while rospy.Time.now() - start < rospy.Duration(10): + if self._recent_response is not None: + break + + if not self._recent_response: + raise RuntimeError("No response from the board within 10 seconds.") + + response = GetValveResponse( + opened=self._recent_response.values & (1 << req.actuator), + ) + self._recent_response = None + return response + + def on_data(self, packet: ActuatorPollResponsePacket | AckPacket) -> None: + """ + Process data received from board. + """ + if isinstance(packet, ActuatorPollResponsePacket): + self._recent_response = packet diff --git a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py new file mode 100644 index 000000000..9a146f363 --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py @@ -0,0 +1,50 @@ +from dataclasses import dataclass + +from mil_usb_to_can.sub9 import Packet + + +@dataclass +class ActuatorSetPacket(Packet, msg_id=0x03, subclass_id=0x00, payload_format="BB"): + """ + Packet used by the actuator board to set a specific valve. + + Attributes: + address (int): The actuator ID to set. + open (bool): Whether to open the specified actuator. ``True`` requests opening, + ``False`` requests closing. + """ + + address: int + open: bool + + +@dataclass +class ActuatorPollRequestPacket( + Packet, + msg_id=0x03, + subclass_id=0x01, + payload_format="", +): + """ + Packet used by the actuator board to request the status of all valves. + """ + + pass + + +@dataclass +class ActuatorPollResponsePacket( + Packet, + msg_id=0x03, + subclass_id=0x02, + payload_format="B", +): + """ + Packet used by the actuator board to return the status of all valves. + + Attributes: + values (int): The statues of all actuators. Bits 0-3 represent the opened + status of actuators 0-3. + """ + + values: int diff --git a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/simulation.py b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/simulation.py new file mode 100644 index 000000000..dd77921e1 --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/simulation.py @@ -0,0 +1,55 @@ +#!/usr/bin/python3 +from __future__ import annotations + +import random + +import rospy +from mil_usb_to_can.sub9 import AckPacket, SimulatedCANDeviceHandle + +from .packets import ( + ActuatorPollRequestPacket, + ActuatorPollResponsePacket, + ActuatorSetPacket, +) + + +class ActuatorBoardSimulation(SimulatedCANDeviceHandle): + """ + Simulator for the communication of the actuator board. + + Attributes: + status (Dict[int, bool]): The status of the valves. The keys are each of the valve IDs, + and the values are the statues of whether the valves are open. + """ + + def __init__(self, *args, **kwargs): + # Tracks the status of the 12 valves + self.status = {i: False for i in range(4)} + super().__init__(*args, **kwargs) + + def on_data(self, packet: ActuatorSetPacket | ActuatorPollRequestPacket) -> None: + """ + Processes data received from motherboard / other devices. For each message received, + the class' status attribute is updated if the message is asking to write, otherwise + a feedback message is constructed and sent back. + """ + # If message is writing a valve, store this change in the internal dictionary + if isinstance(packet, ActuatorSetPacket): + rospy.sleep(random.randrange(0, 1)) # Time to simluate opening of valves + self.status[packet.address] = packet.open + self.send_data(bytes(AckPacket())) + + # If message is a status request, send motherboard the status of the requested valve + elif isinstance(packet, ActuatorPollRequestPacket): + self.send_data( + bytes( + ActuatorPollResponsePacket( + int( + "".join( + str(int(x)) for x in reversed(self.status.values()) + ), + base=2, + ), + ), + ), + ) diff --git a/SubjuGator/drivers/sub_actuator_board/test/simulated_board.test b/SubjuGator/drivers/sub_actuator_board/test/simulated_board.test new file mode 100644 index 000000000..7257f68ef --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/test/simulated_board.test @@ -0,0 +1,15 @@ + + + + + + + device_handles: + sub_actuator_board.ActuatorBoard: [sub_actuator_board.ActuatorPollResponsePacket] + simulated_devices: + sub_actuator_board.ActuatorBoardSimulation: [sub_actuator_board.ActuatorPollRequestPacket, sub_actuator_board.ActuatorSetPacket] + + + + + diff --git a/SubjuGator/drivers/sub_actuator_board/test/simulated_board_test.py b/SubjuGator/drivers/sub_actuator_board/test/simulated_board_test.py new file mode 100755 index 000000000..8c4665f09 --- /dev/null +++ b/SubjuGator/drivers/sub_actuator_board/test/simulated_board_test.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python3 +import unittest + +import rospy +from sub_actuator_board.srv import GetValve, GetValveRequest, SetValve, SetValveRequest + + +class SimulatedBoardTest(unittest.TestCase): + """ + Integration test for CAN2USB board driver. Talks + to a simulated CAN device which should add two integers + """ + + def __init__(self, *args): + self.set_srv = rospy.ServiceProxy("/set_valve", SetValve) + self.get_srv = rospy.ServiceProxy("/get_valve", GetValve) + super().__init__(*args) + + def test_correct_response(self): + """ + Test we can get correct data through CAN bus at least ten times. + """ + self.set_srv.wait_for_service(1) + self.get_srv.wait_for_service(1) + self.assertTrue(self.set_srv(SetValveRequest(0, True)).success) + self.assertTrue(self.get_srv(GetValveRequest(0)).opened) + self.assertTrue(self.set_srv(SetValveRequest(0, False)).success) + self.assertFalse(self.get_srv(GetValveRequest(0)).opened) + self.assertTrue(self.set_srv(SetValveRequest(0, False)).success) + self.assertFalse(self.get_srv(GetValveRequest(0)).opened) + self.assertTrue(self.set_srv(SetValveRequest(3, True)).success) + self.assertTrue(self.get_srv(GetValveRequest(3)).opened) + self.assertFalse(self.get_srv(GetValveRequest(0)).opened) + self.assertTrue(self.set_srv(SetValveRequest(4, False)).success) + self.assertFalse(self.get_srv(GetValveRequest(4)).opened) + self.assertFalse(self.get_srv(GetValveRequest(0)).opened) + + +if __name__ == "__main__": + rospy.init_node("simulated_board_test", anonymous=True) + import rostest + + rostest.rosrun("sub_actuator_board", "simulated_board_test", SimulatedBoardTest) + unittest.main() diff --git a/SubjuGator/etc/motd b/SubjuGator/etc/motd new file mode 100644 index 000000000..bf6bb0855 --- /dev/null +++ b/SubjuGator/etc/motd @@ -0,0 +1,9 @@ +#-------#-------#-------#-------#-------#-------#-------#-------# + You have connected to SubjuGator 8! +#-------#-------#-------#-------#-------#-------#-------#-------# + +If you are testing the sub in the water (or are preparing to do so), please make sure to read the Subjugator Testing Procedures page on uf-mil.github.io for useful tips, advice, and checklists. + +We (the previous MIL members) will watch your testing session with great interest. + +Happy Testing! diff --git a/SubjuGator/gnc/subjugator_thruster_mapper/config/thruster_layout.yaml b/SubjuGator/gnc/subjugator_thruster_mapper/config/thruster_layout.yaml index 2d3de8ac9..48992559a 100644 --- a/SubjuGator/gnc/subjugator_thruster_mapper/config/thruster_layout.yaml +++ b/SubjuGator/gnc/subjugator_thruster_mapper/config/thruster_layout.yaml @@ -30,7 +30,7 @@ thruster_ports: # mapping from thrust in Newtons to effort (e = t[0]x^4 + t[1]x^3 + t[2]x^2 + t[3]x) thrusters: FLH: { - node_id: 3, + node_id: 4, position: [0.2678, 0.2795, 0.0], direction: [0.866, -0.5, 0.0], thrust_bounds: [-81.85, 99.7], @@ -42,7 +42,7 @@ thrusters: } } FLV: { - node_id: 1, + node_id: 5, position: [0.1583, 0.169, 0.0142], direction: [0.0, 0.0, 1.0], thrust_bounds: [-81.85, 99.7], @@ -54,7 +54,7 @@ thrusters: } } FRH: { - node_id: 2, + node_id: 0, position: [0.2678, -0.2795, 0.0], direction: [0.866, 0.5, 0.0], thrust_bounds: [-81.85, 99.7], @@ -66,9 +66,9 @@ thrusters: } } FRV: { - node_id: 0, + node_id: 1, position: [0.1583, -0.169, 0.0142], - direction: [0.0, 0.0, -1.0], + direction: [0.0, 0.0, 1.0], thrust_bounds: [-81.85, 99.7], calib: { # yamllint disable-line rule:line-length @@ -78,7 +78,7 @@ thrusters: } } BLH: { - node_id: 4, + node_id: 3, position: [-0.2678, 0.2795, 0.0], direction: [0.866, 0.5, 0.0], thrust_bounds: [-81.85, 99.7], @@ -90,7 +90,7 @@ thrusters: } } BLV: { - node_id: 5, + node_id: 6, position: [-0.1583, 0.169, 0.0142], direction: [0.0, 0.0, 1.0], thrust_bounds: [-81.85, 99.7], @@ -102,7 +102,7 @@ thrusters: } } BRH: { - node_id: 6, + node_id: 7, position: [-0.2678, -0.2795, 0.0], direction: [0.866, -0.5, 0.0], thrust_bounds: [-81.85, 99.7], @@ -114,7 +114,7 @@ thrusters: } } BRV: { - node_id: 7, + node_id: 2, position: [-0.1583, -0.169, 0.0142], direction: [0.0, 0.0, 1.0], thrust_bounds: [-81.85, 99.7], diff --git a/SubjuGator/perception/subjugator_perception/ml_classifiers/path_marker/path_localizer.py b/SubjuGator/perception/subjugator_perception/ml_classifiers/path_marker/path_localizer.py index 29665f12a..c70e5e258 100755 --- a/SubjuGator/perception/subjugator_perception/ml_classifiers/path_marker/path_localizer.py +++ b/SubjuGator/perception/subjugator_perception/ml_classifiers/path_marker/path_localizer.py @@ -17,7 +17,7 @@ rospack.get_path("subjugator_perception") + "/ml_classifiers/path_marker/utils", ) -from utils import detector_utils # noqa: manually appending sys.path +from utils import detector_utils # noqa class classifier: diff --git a/SubjuGator/perception/subjugator_perception/nodes/dice_detect.py b/SubjuGator/perception/subjugator_perception/nodes/dice_detect.py index 55d1dd5e2..312123597 100755 --- a/SubjuGator/perception/subjugator_perception/nodes/dice_detect.py +++ b/SubjuGator/perception/subjugator_perception/nodes/dice_detect.py @@ -93,7 +93,7 @@ def detect(self, dice_img): params.minConvexity = 0.8 # 1 = perfect convex hull # Filter by Inertia params.filterByInertia = True - params.minInertiaRatio = 0.4 # Defines the ellipsoid 1= detects only cirlces + params.minInertiaRatio = 0.4 # Defines the ellipsoid 1= detects only circles # 0 = Detects even lines # Create a detector with the parameters diff --git a/SubjuGator/perception/subjugator_perception/nodes/hsv_calibration.py b/SubjuGator/perception/subjugator_perception/nodes/hsv_calibration.py index 4a67f4bb7..433133431 100755 --- a/SubjuGator/perception/subjugator_perception/nodes/hsv_calibration.py +++ b/SubjuGator/perception/subjugator_perception/nodes/hsv_calibration.py @@ -36,13 +36,17 @@ def parse_string(threshes): def reconfigure(self, config, level): try: self.lower = np.array(self.parse_string(config["dyn_lower"])) - rospy.logwarn("HSV lower bound below minimum value") if ( - self.lower < 0 - ).any() else None + ( + rospy.logwarn("HSV lower bound below minimum value") + if (self.lower < 0).any() + else None + ) self.upper = np.array(self.parse_string(config["dyn_upper"])) - rospy.logwarn("HSV upper bound above maximum values") if ( - self.upper[0] > 179 - ).any() or (self.upper[1:] > 255).any() else None + ( + rospy.logwarn("HSV upper bound above maximum values") + if (self.upper[0] > 179).any() or (self.upper[1:] > 255).any() + else None + ) except ValueError as e: rospy.logwarn(f"Invalid dynamic reconfigure: {e}") diff --git a/SubjuGator/perception/subjugator_perception/nodes/orange_rectangle_finder.py b/SubjuGator/perception/subjugator_perception/nodes/orange_rectangle_finder.py index 0701c3b05..237fe94e3 100755 --- a/SubjuGator/perception/subjugator_perception/nodes/orange_rectangle_finder.py +++ b/SubjuGator/perception/subjugator_perception/nodes/orange_rectangle_finder.py @@ -52,7 +52,7 @@ class OrangeRectangleFinder: * Transform this frames pose into /map frame * Plug this frames pose in /map into a kalman filter to reduce noise - TODO: Allow for two objects to be identifed at once, both filtered through its own KF + TODO: Allow for two objects to be identified at once, both filtered through its own KF """ # Coordinate axes for debugging image diff --git a/SubjuGator/perception/subjugator_perception/nodes/torpedo_board.cpp b/SubjuGator/perception/subjugator_perception/nodes/torpedo_board.cpp index a5f2aa352..51f57e387 100644 --- a/SubjuGator/perception/subjugator_perception/nodes/torpedo_board.cpp +++ b/SubjuGator/perception/subjugator_perception/nodes/torpedo_board.cpp @@ -1494,7 +1494,7 @@ void anisotropic_diffusion(const Mat &src, Mat &dest, int t_max) if (i == 0 || i == x0.rows - 1 || j == 0 || j == x0.cols - 1) // conduction coefficient set to d = 1; // 1 p633 after equation 13 else - d = 1.0 / (1 + (gx * gx + 0 * gy * gy) * K2); // expression of g(gradient(I)) + d = 1.0 / (1 + (gx * gx + 0 * gy * gy) * K2); // expression of g(gradient(I)) // d =-exp(-(gx*gx+gy*gy)*K2); // expression of g(gradient(I)) D.at(i, j) = d; } diff --git a/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/HOG/HOG_SVM_trainer.py b/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/HOG/HOG_SVM_trainer.py index bbdb7aefb..01e85b3a2 100644 --- a/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/HOG/HOG_SVM_trainer.py +++ b/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/HOG/HOG_SVM_trainer.py @@ -16,8 +16,7 @@ class_list = np.array(0) # loop through the training images -count = 0 -for i in os.listdir(os.path.abspath(folder)): +for count, i in enumerate(os.listdir(os.path.abspath(folder))): # get the roi from the file path = folder + "/" + i roi_str = f.readline() @@ -72,7 +71,6 @@ class_temp = np.empty(len_myinvhog / 9) class_temp.fill(-1) class_list = np.append(class_list, class_temp) - count += 1 # hack class_list = np.delete(class_list, [0]) diff --git a/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/machine_learning/boost_auto.py b/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/machine_learning/boost_auto.py index c9027ff60..c1cf17e1a 100644 --- a/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/machine_learning/boost_auto.py +++ b/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/machine_learning/boost_auto.py @@ -100,10 +100,11 @@ def train_on_data(observation_list, label_list, split_factor=4): s_time = time.time() process_round = 0 # Split this into multiple passes in an attempt to free RAM (no idea if this works). - for x, y in zip(all_observations_split, all_labels_split): + for process_round, (x, y) in enumerate( + zip(all_observations_split, all_labels_split), + ): print(f"Training subset {process_round + 1}/{split_factor}.") boost.train(x, cv2.CV_ROW_SAMPLE, y, params=parameters) - process_round += 1 print(f"Time to complete: {time.time() - s_time}") print("Done! Saving...") diff --git a/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/marker_occ_grid.py b/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/marker_occ_grid.py index 9261de0ac..c547f7847 100644 --- a/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/marker_occ_grid.py +++ b/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/marker_occ_grid.py @@ -174,7 +174,7 @@ def return_pose(self, srv): self.poly_generator = self.polygon_generator() return [0, False, srv.intial_position] - # We search at 1.5 * r so that there is some overlay in the search feilds. + # We search at 1.5 * r so that there is some overlay in the search fields. np_pose = msg_helpers.pose_to_numpy(srv.intial_position) rot_mat = make_2D_rotation( tf.transformations.euler_from_quaternion(np_pose[1])[2], @@ -249,7 +249,6 @@ def polygon_generator(self, n=12): class MarkerOccGrid(OccGridUtils): - """ Handles updating occupancy grid when new data comes in. TODO: Upon call can return some path to go to in order to find them. diff --git a/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/rviz.py b/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/rviz.py index c46dd18ea..e6af8929a 100644 --- a/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/rviz.py +++ b/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/rviz.py @@ -9,7 +9,6 @@ class RvizVisualizer: - """Cute tool for drawing both depth and height-from-bottom in RVIZ""" def __init__(self, topic="visualization/markers"): diff --git a/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/visual_threshold_tools.py b/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/visual_threshold_tools.py index 7ec7d04c9..f30275cfc 100644 --- a/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/visual_threshold_tools.py +++ b/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/visual_threshold_tools.py @@ -63,7 +63,6 @@ def denormalize(val, _min, _max): class ExtentDialog(traits.api.HasTraits): - """A dialog to graphically adjust the extents of a threshold range""" # Data extents diff --git a/SubjuGator/perception/subjugator_perception/test/test_path_marker.py b/SubjuGator/perception/subjugator_perception/test/test_path_marker.py index 4f3a621cc..5674ddf5a 100755 --- a/SubjuGator/perception/subjugator_perception/test/test_path_marker.py +++ b/SubjuGator/perception/subjugator_perception/test/test_path_marker.py @@ -17,7 +17,6 @@ class TestPathMarker(unittest.TestCase): - """ Unit test for perception service finding orange path markers Plays several bags with known pixel coordinates of path marker, diff --git a/SubjuGator/simulation/subjugator_gazebo/diagnostics/gazebo_tests/common.py b/SubjuGator/simulation/subjugator_gazebo/diagnostics/gazebo_tests/common.py index de7bc39f0..38a5baf41 100644 --- a/SubjuGator/simulation/subjugator_gazebo/diagnostics/gazebo_tests/common.py +++ b/SubjuGator/simulation/subjugator_gazebo/diagnostics/gazebo_tests/common.py @@ -8,7 +8,6 @@ class Job: - """Inherit from this!""" _job_name = "generic job" diff --git a/SubjuGator/simulation/subjugator_simulation/subjugator_sim_tools/shaders/shader_manager.py b/SubjuGator/simulation/subjugator_simulation/subjugator_sim_tools/shaders/shader_manager.py index edfb7d12f..192755207 100644 --- a/SubjuGator/simulation/subjugator_simulation/subjugator_sim_tools/shaders/shader_manager.py +++ b/SubjuGator/simulation/subjugator_simulation/subjugator_sim_tools/shaders/shader_manager.py @@ -36,7 +36,6 @@ def add_item(self, position, intensity): class ShaderManager: - """ This class is used when you want to have an entity switch out shaders at a specific time. If you want to add another rule for when to switch out shaders: diff --git a/SubjuGator/simulation/subjugator_simulation/subjugator_sim_tools/shaders/shaders.py b/SubjuGator/simulation/subjugator_simulation/subjugator_sim_tools/shaders/shaders.py index d4a2e1f62..210a68e8e 100644 --- a/SubjuGator/simulation/subjugator_simulation/subjugator_sim_tools/shaders/shaders.py +++ b/SubjuGator/simulation/subjugator_simulation/subjugator_sim_tools/shaders/shaders.py @@ -65,7 +65,6 @@ def recursive_dictionary(self, path, text): class Shaders: - """A Shaders class, which automatically contains all of the shaders in the shaders folder WTF? diff --git a/SubjuGator/simulation/subjugator_simulation/subjugator_sim_tools/widgets/sub.py b/SubjuGator/simulation/subjugator_simulation/subjugator_sim_tools/widgets/sub.py index cbb628bad..144ec195d 100644 --- a/SubjuGator/simulation/subjugator_simulation/subjugator_sim_tools/widgets/sub.py +++ b/SubjuGator/simulation/subjugator_simulation/subjugator_sim_tools/widgets/sub.py @@ -83,7 +83,6 @@ def make_visual(self, physical, position, thrust_indicators=False): ) class ThrustGetter: - """This is a pretty garbage thing, and will be replaced by the scenegraph system""" def __init__(self, thruster_name, rdir): diff --git a/SubjuGator/utils/subjugator_build_tools/include/subjugator_build_tools/backward.hpp b/SubjuGator/utils/subjugator_build_tools/include/subjugator_build_tools/backward.hpp index 9c6f0316e..35f85d221 100644 --- a/SubjuGator/utils/subjugator_build_tools/include/subjugator_build_tools/backward.hpp +++ b/SubjuGator/utils/subjugator_build_tools/include/subjugator_build_tools/backward.hpp @@ -222,7 +222,7 @@ struct hashtable using std::move; } // namespace details } // namespace backward -#else // NOT BACKWARD_ATLEAST_CXX11 +#else // NOT BACKWARD_ATLEAST_CXX11 #include namespace backward { @@ -752,7 +752,7 @@ class StackTraceImpl : public StackTraceLinuxImplHolder }; }; -#else // BACKWARD_HAS_UNWIND == 0 +#else // BACKWARD_HAS_UNWIND == 0 template <> class StackTraceImpl : public StackTraceLinuxImplHolder @@ -1902,7 +1902,7 @@ class Colorize bool _istty; }; -#else // ndef BACKWARD_SYSTEM_LINUX +#else // ndef BACKWARD_SYSTEM_LINUX namespace Color { @@ -2152,7 +2152,7 @@ class SignalHandling StackTrace st; void* error_addr = 0; -#ifdef REG_RIP // x86_64 +#ifdef REG_RIP // x86_64 error_addr = reinterpret_cast(uctx->uc_mcontext.gregs[REG_RIP]); #elif defined(REG_EIP) // x86_32 error_addr = reinterpret_cast(uctx->uc_mcontext.gregs[REG_EIP]); diff --git a/SubjuGator/utils/subjugator_diagnostics/scripts/self_check.py b/SubjuGator/utils/subjugator_diagnostics/scripts/self_check.py index 2552541cc..4db32b96c 100755 --- a/SubjuGator/utils/subjugator_diagnostics/scripts/self_check.py +++ b/SubjuGator/utils/subjugator_diagnostics/scripts/self_check.py @@ -18,7 +18,6 @@ class TemplateChecker: - """Template for how each checker class should look. This provides interface functions for the main function to use. You don't have to implement them all in each checker. diff --git a/docs/design/passive_sonar/passive_sonar.rst b/docs/design/passive_sonar/passive_sonar.rst index 522368705..6f23f38ab 100644 --- a/docs/design/passive_sonar/passive_sonar.rst +++ b/docs/design/passive_sonar/passive_sonar.rst @@ -78,8 +78,8 @@ Configuration ************* All parameters that are expected to be changed in tuining are ROS Params initialized in ``passive_sonar.yaml`` -To make a custom configation -^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +To make a custom configuration +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ * ``roscd subjugator_launch`` diff --git a/docs/electrical/onboarding.md b/docs/electrical/onboarding.md index c48026aaa..45d73e7a7 100644 --- a/docs/electrical/onboarding.md +++ b/docs/electrical/onboarding.md @@ -20,7 +20,7 @@ including meeting times. ## Join the electrical GitHub Unlike the software and mechanical teams (who share a GitHub repository), we use -a separate repository for the electrical team. As a resut, you will need to be added +a separate repository for the electrical team. As a result, you will need to be added to this organization. Once you're in Slack (or you've come to the lab), ask an electrical leader to add you to the GitHub organization. The organization can be found [here](https://github.com/uf-mil-electrical). diff --git a/docs/extensions/attributetable.py b/docs/extensions/attributetable.py index abc041ace..53372cb61 100644 --- a/docs/extensions/attributetable.py +++ b/docs/extensions/attributetable.py @@ -2,6 +2,7 @@ Credit goes to discord.py and its creators for creating most of this file: https://github.com/Rapptz/discord.py/blob/master/docs/extensions/attributetable.py """ + import importlib import inspect import re @@ -340,7 +341,7 @@ def process_cppattributetable(app, doctree: Node, fromdocname): ], ) - elif all([c in node.attributes["classes"] for c in ["cpp", "function"]]): + elif all(c in node.attributes["classes"] for c in ["cpp", "function"]): # Get the signature line of the function, where its name is stored try: descriptions = [ diff --git a/docs/extensions/builder.py b/docs/extensions/builder.py index 84de6d59d..881417afc 100644 --- a/docs/extensions/builder.py +++ b/docs/extensions/builder.py @@ -2,6 +2,7 @@ Credit goes to discord.py and its creators for creating most of this file: https://github.com/Rapptz/discord.py/blob/master/docs/extensions/attributetable.py """ + from sphinx.builders.html import StandaloneHTMLBuilder from sphinx.environment.adapters.indexentries import IndexEntries from sphinx.writers.html5 import HTML5Translator diff --git a/docs/images/gazebo/Bottom_Toolbar.png b/docs/images/gazebo/Bottom_Toolbar.png new file mode 100644 index 000000000..de1fb6d98 Binary files /dev/null and b/docs/images/gazebo/Bottom_Toolbar.png differ diff --git a/docs/images/gazebo/Interface.png b/docs/images/gazebo/Interface.png new file mode 100644 index 000000000..b6f5018c4 Binary files /dev/null and b/docs/images/gazebo/Interface.png differ diff --git a/docs/images/gazebo/Labeled_Interface.png b/docs/images/gazebo/Labeled_Interface.png new file mode 100644 index 000000000..871ddca52 Binary files /dev/null and b/docs/images/gazebo/Labeled_Interface.png differ diff --git a/docs/images/gazebo/Menus.png b/docs/images/gazebo/Menus.png new file mode 100644 index 000000000..d6c39d62e Binary files /dev/null and b/docs/images/gazebo/Menus.png differ diff --git a/docs/images/gazebo/Model_Editor.png b/docs/images/gazebo/Model_Editor.png new file mode 100644 index 000000000..362f7cb16 Binary files /dev/null and b/docs/images/gazebo/Model_Editor.png differ diff --git a/docs/images/gazebo/Mouse.png b/docs/images/gazebo/Mouse.png new file mode 100644 index 000000000..728c8fc8f Binary files /dev/null and b/docs/images/gazebo/Mouse.png differ diff --git a/docs/images/gazebo/TrackPad.png b/docs/images/gazebo/TrackPad.png new file mode 100644 index 000000000..1231be57b Binary files /dev/null and b/docs/images/gazebo/TrackPad.png differ diff --git a/docs/images/gazebo/Upper_Toolbar.png b/docs/images/gazebo/Upper_Toolbar.png new file mode 100644 index 000000000..68e36e69a Binary files /dev/null and b/docs/images/gazebo/Upper_Toolbar.png differ diff --git a/docs/indyav/software/planning/indyav_path/path_recorder.rst b/docs/indyav/software/planning/indyav_path/path_recorder.rst index 77c3d4ee3..d1b5f1a47 100644 --- a/docs/indyav/software/planning/indyav_path/path_recorder.rst +++ b/docs/indyav/software/planning/indyav_path/path_recorder.rst @@ -33,7 +33,7 @@ Basic Usage Example - Visualize this new topic in rviz by clicking `Add` and then `/odom2 -> Odometry` -- Watch the red arrow advance from where the sub started to where the sub stopped updting approximately 10 times a second. +- Watch the red arrow advance from where the sub started to where the sub stopped updating approximately 10 times a second. Source Files ^^^^^^^^^^^^ diff --git a/docs/infrastructure/shipping.md b/docs/infrastructure/shipping.md index 068280f14..d0636bf21 100644 --- a/docs/infrastructure/shipping.md +++ b/docs/infrastructure/shipping.md @@ -34,3 +34,10 @@ ion base, many shipping carriers won't take batteries without special precaution You will need to review these in order to ship them through standard shipping. For some locations, it may be easier to buy batteries at the competition site rather than to take batteries with you. + +## Note + +Whenever a package is received in MIL, an email needs to be sent to Dr. Schwartz. +The email should contain the sender's name of the package, and a picture of the +packing receipt, if one is included. This should be done by the lab member who +opens up the package. diff --git a/docs/navigator/lessons22.md b/docs/navigator/lessons22.md index 1c92fc298..25607540e 100644 --- a/docs/navigator/lessons22.md +++ b/docs/navigator/lessons22.md @@ -62,7 +62,7 @@ future teams to learn from! It is quicker to tell when tools have gone missing (as each tool now has a dedicated spot in the organizer), and which tool is missing. Furthermore, it becomes easier for software members, electrical members, and mentors to find - tools, even if they maybe are not acquianted with a deep knowledge of the + tools, even if they maybe are not acquainted with a deep knowledge of the mechanical team. * **Focus on bringing less and staying more organized.** - At RobotX 2022, we were one of the teams who had brought the most equipment, yet we had a hard diff --git a/docs/reference/bagging.rst b/docs/reference/bagging.rst new file mode 100644 index 000000000..feb007dad --- /dev/null +++ b/docs/reference/bagging.rst @@ -0,0 +1,9 @@ +Online Bagger +------------- + +.. currentmodule:: mil_tools + +.. attributetable:: nodes.online_bagger.OnlineBagger + +.. autoclass:: nodes.online_bagger.OnlineBagger + :members: diff --git a/docs/reference/can.rst b/docs/reference/can.rst index 22d1f0cd4..817163f78 100644 --- a/docs/reference/can.rst +++ b/docs/reference/can.rst @@ -3,8 +3,13 @@ .. automodule:: mil_usb_to_can +:mod:`mil_usb_to_can.sub8` - SubjuGator 8 +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +.. automodule:: mil_usb_to_can.sub8 + Packet Format -^^^^^^^^^^^^^ +~~~~~~~~~~~~~ In order to reliably communicate with the USB to CAN board, a consistent packet format must be defined. @@ -73,7 +78,7 @@ other devices. - The checksum for the data. Checksums -^^^^^^^^^ +~~~~~~~~~ All messages contain a checksum to help verify data integrity. However, receiving packets also have a special byte containing a slightly modified checksum formula. @@ -81,11 +86,11 @@ The checksum in all packets is found by adding up all bytes in the byte string, including the start/end flags, and then using modulo 16 on this result. Exceptions -^^^^^^^^^^ +~~~~~~~~~~ Exception Hierarchy -~~~~~~~~~~~~~~~~~~~ -.. currentmodule:: mil_usb_to_can +""""""""""""""""""" +.. currentmodule:: mil_usb_to_can.sub8 .. exception_hierarchy:: @@ -99,7 +104,7 @@ Exception Hierarchy - :exc:`InvalidEndFlagException` Exception List -~~~~~~~~~~~~~~~~~~~ +""""""""""""""""""" .. autoclass:: ApplicationPacketWrongIdentifierException :members: @@ -122,64 +127,200 @@ Exception List :members: ApplicationPacket -^^^^^^^^^^^^^^^^^ -.. attributetable:: mil_usb_to_can.ApplicationPacket +~~~~~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.ApplicationPacket -.. autoclass:: mil_usb_to_can.ApplicationPacket +.. autoclass:: mil_usb_to_can.sub8.ApplicationPacket :members: USBtoCANBoard -^^^^^^^^^^^^^ -.. attributetable:: mil_usb_to_can.USBtoCANBoard +~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.USBtoCANBoard -.. autoclass:: mil_usb_to_can.USBtoCANBoard +.. autoclass:: mil_usb_to_can.sub8.USBtoCANBoard :members: CANDeviceHandle -^^^^^^^^^^^^^^^ -.. attributetable:: mil_usb_to_can.CANDeviceHandle +~~~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.CANDeviceHandle -.. autoclass:: mil_usb_to_can.CANDeviceHandle +.. autoclass:: mil_usb_to_can.sub8.CANDeviceHandle :members: USBtoCANDriver -^^^^^^^^^^^^^^ -.. attributetable:: mil_usb_to_can.USBtoCANDriver +~~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.USBtoCANDriver -.. autoclass:: mil_usb_to_can.USBtoCANDriver +.. autoclass:: mil_usb_to_can.sub8.USBtoCANDriver :members: SimulatedCANDevice -^^^^^^^^^^^^^^^^^^ -.. attributetable:: mil_usb_to_can.SimulatedCANDevice +~~~~~~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.SimulatedCANDevice -.. autoclass:: mil_usb_to_can.SimulatedCANDevice +.. autoclass:: mil_usb_to_can.sub8.SimulatedCANDevice :members: SimulatedUSBtoCAN -^^^^^^^^^^^^^^^^^ -.. attributetable:: mil_usb_to_can.SimulatedUSBtoCAN +~~~~~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.SimulatedUSBtoCAN -.. autoclass:: mil_usb_to_can.SimulatedUSBtoCAN +.. autoclass:: mil_usb_to_can.sub8.SimulatedUSBtoCAN :members: Packet -^^^^^^ -.. attributetable:: mil_usb_to_can.Packet +~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.Packet -.. autoclass:: mil_usb_to_can.Packet +.. autoclass:: mil_usb_to_can.sub8.Packet :members: ReceivePacket -^^^^^^^^^^^^^ -.. attributetable:: mil_usb_to_can.ReceivePacket +~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.ReceivePacket -.. autoclass:: mil_usb_to_can.ReceivePacket +.. autoclass:: mil_usb_to_can.sub8.ReceivePacket :members: CommandPacket -^^^^^^^^^^^^^ -.. attributetable:: mil_usb_to_can.CommandPacket +~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub8.CommandPacket + +.. autoclass:: mil_usb_to_can.sub8.CommandPacket + :members: + +:mod:`mil_usb_to_can.sub9` - SubjuGator 9 +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +.. automodule:: mil_usb_to_can.sub9 +.. currentmodule:: mil_usb_to_can.sub9 + +Packet Format +~~~~~~~~~~~~~ +In order to reliably communicate with the USB to CAN board, a consistent packet format +must be defined. + +Below is the USBtoCAN Packet Format. This packet format wraps every message being +sent over the serial connection to the USB to CAN board from ROS. + +.. list-table:: USBtoCAN Packet Format + :header-rows: 1 + + * - Name + - Length + - Description + * - Sync character 1 (``0x37``) + - 1 byte + - First sync character indicating the start of packets. + * - Sync character 2 (``0x01``) + - 1 byte + - Second sync character indicating the start of packets. + * - Class ID + - 1 byte + - Message class. Determines the family of messages the packet belongs to. + * - Subclass ID + - 1 byte + - Message subclass. In combination with class, determines specific qualities + of message. + * - Payload Length + - 2 bytes + - Length of payload. + * - Payload + - 0-65535 bytes + - Payload. Meaning of payload is determined by specific packet class/subclass. + * - Checksum A + - 1 byte + - First byte of Fletcher's checksum. + * - Checksum B + - 1 byte + - Second byte of Fletcher's checksum. + +Checksums +~~~~~~~~~ +All messages contain a checksum to help verify data integrity. However, receiving +packets also have a special byte containing a slightly modified checksum formula. + +The checksum in all packets is found by adding up all bytes in the byte string, +including the start/end flags, and then using modulo 16 on this result. + +Packet Listing +~~~~~~~~~~~~~~ +Below is a listing of all available packets. The payload format is the format +used by the :mod:`struct` module. For more information, see the Python documentation +on the :ref:`list of format characters`, and their corresponding +byte length. + ++------------+--------------+----------------+-------------------------------------------------------------------------+ +| Message ID | Subclass ID | Payload Format | Class | ++============+==============+================+=========================================================================+ +| 0x00 | 0x00 | Empty | :class:`mil_usb_to_can.sub9.NackPacket` | ++ (Meta) +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x01 | Empty | :class:`mil_usb_to_can.sub9.AckPacket` | ++------------+--------------+----------------+-------------------------------------------------------------------------+ +| 0x01 | 0x00 | Empty | :class:`sub8_thrust_and_kill_board.HeartbeatPacket` | ++ (Sub8 +--------------+----------------+-------------------------------------------------------------------------+ +| Thrust/ | 0x01 | ``Bf`` | :class:`sub8_thrust_and_kill_board.ThrustSetPacket` | ++ Kill) +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x02 | ``B`` | :class:`sub8_thrust_and_kill_board.KillSetPacket` | ++ +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x03 | ``B`` | :class:`sub8_thrust_and_kill_board.KillReceivePacket` | ++------------+--------------+----------------+-------------------------------------------------------------------------+ +| 0x02 | 0x00 | Empty | :class:`sub9_thrust_and_kill_board.HeartbeatSetPacket` | ++ (Sub9 +--------------+----------------+-------------------------------------------------------------------------+ +| Thrust/ | 0x01 | Empty | :class:`sub9_thrust_and_kill_board.HeartbeatReceivePacket` | ++ Kill) +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x02 | ``Bf`` | :class:`sub9_thrust_and_kill_board.ThrustSetPacket` | ++ +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x03 | ``B`` | :class:`sub9_thrust_and_kill_board.KillSetPacket` | ++ +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x04 | ``B`` | :class:`sub9_thrust_and_kill_board.KillReceivePacket` | ++------------+--------------+----------------+-------------------------------------------------------------------------+ +| 0x03 | 0x00 | Empty | :class:`sub8_battery_monitor_board.BatteryPollRequestPacket` | ++ (Battery +--------------+----------------+-------------------------------------------------------------------------+ +| Monitor) | 0x01 | ``ffff`` | :class:`sub8_battery_monitor_board.BatteryPollResponsePacket` | ++------------+--------------+----------------+-------------------------------------------------------------------------+ +| 0x04 | 0x00 | ``BB`` | :class:`sub_actuator_board.ActuatorSetPacket` | ++ (Actuator +--------------+----------------+-------------------------------------------------------------------------+ +| Board) | 0x01 | Empty | :class:`sub_actuator_board.ActuatorPollRequestPacket` | ++ +--------------+----------------+-------------------------------------------------------------------------+ +| | 0x02 | ``B`` | :class:`sub_actuator_board.ActuatorPollResponsePacket` | ++------------+--------------+----------------+-------------------------------------------------------------------------+ +| 0x05 | 0x00 | Empty | :class:`sub9_system_status_board.SetLedRequestPacket` | +| (System | | | | +| Status) | | | | ++------------+--------------+----------------+-------------------------------------------------------------------------+ + +CANDeviceHandle +~~~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub9.CANDeviceHandle + +.. autoclass:: mil_usb_to_can.sub9.CANDeviceHandle + :members: + +SimulatedCANDeviceHandle +~~~~~~~~~~~~~~~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub9.SimulatedCANDeviceHandle + +.. autoclass:: mil_usb_to_can.sub9.SimulatedCANDeviceHandle + :members: + +Packet +~~~~~~ +.. attributetable:: mil_usb_to_can.sub9.Packet + +.. autoclass:: mil_usb_to_can.sub9.Packet + :members: + +NackPacket +~~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub9.NackPacket + +.. autoclass:: mil_usb_to_can.sub9.NackPacket + :members: + +AckPacket +~~~~~~~~~ +.. attributetable:: mil_usb_to_can.sub9.AckPacket -.. autoclass:: mil_usb_to_can.CommandPacket +.. autoclass:: mil_usb_to_can.sub9.AckPacket :members: diff --git a/docs/reference/index.rst b/docs/reference/index.rst index b6f96f49e..c63581a0c 100644 --- a/docs/reference/index.rst +++ b/docs/reference/index.rst @@ -31,3 +31,4 @@ by MIL. These subsystems relate to a variety of processes. poi pneumatic sabertooth + bagging diff --git a/docs/software/adding_documentation.md b/docs/software/adding_documentation.md index cc1a1fd46..d91bf76e3 100644 --- a/docs/software/adding_documentation.md +++ b/docs/software/adding_documentation.md @@ -100,5 +100,5 @@ $ ./scripts/build_docs --scratch ``` ## Contributing changes -Now that you have made and verifed your changes, follow the [contributing guide](contributing) +Now that you have made and verified your changes, follow the [contributing guide](contributing) to add your changes to the repository. diff --git a/docs/software/alarms.md b/docs/software/alarms.md new file mode 100644 index 000000000..80fb51485 --- /dev/null +++ b/docs/software/alarms.md @@ -0,0 +1,114 @@ +# Alarms in ROS + +In the realm of building dependable control systems, the importance of error detection +and effective error-handling mechanisms cannot be overstated. Within this context, +MIL presents a robust solution in the form of a live alarm system. This alarm system +operates discreetly in the background of both the robot's mission and driver codebases, +ready to be activated upon the emergence of errors. Notably, the alarm code doesn't +solely serve to identify and address errors; it can also adeptly manage changes +or updates that extend beyond error scenarios. + +## ROS Alarms: A Service-Oriented Architecture + +The architecture of ROS alarms distinguishes itself by employing a service-oriented +model rather than the usual topic-based approach. In ROS, Services act as the +conduits for interaction between nodes, functioning in a request-response manner. +While ROS topics enable asynchronous data exchange, services facilitate nodes in +seeking specific actions or information from other nodes, awaiting a subsequent +response before proceeding. This method of waiting before proceeding is known as a +synchronous data exchange. This proves especially valuable in tasks that require +direct engagement, such as data retrieval or computations. + +## Alarm System Logic + +The alarm system's functionality is more intricate than that of a typical ROS +service, which usually manages operations of base types (ints, strings, etc.). +In this scenario, the alarm's service server is engineered to manage the tasks +of updating, querying, and processing an alarm object. ROS alarms encompass two +distinct types of clients: the alarm broadcaster and the alarm listener. The +broadcaster initializes and triggers alarms in response to errors or changes, +while the listener monitors the broadcaster's activity and activates designated +a callback function when alarms are raised. The callback function should handle +the error or change appropriately. + +To successfully leverage alarms, the initialization of both the broadcaster and +listener is needed. The listener should be configured to execute a predefined +callback function, addressing errors or changes detected by the broadcaster. +Within your codebase, error detection and alarm-raising procedures should be +integrated. If orchestrated correctly, the callback function will be automatically +invoked, underscoring successful error mitigation. + +Note that there are several special properties that can be attached to your alarm. +Here are a couple of examples: +* When you raise an alarm you can assign a severity level to the alarm [0, 5]. +* You can attach multiple callback functions to the alarm. + * **This is where severity comes into play!** By specifying the required + severity level that is needed to execute the callback when initializing the + function, you can choose which callbacks are executed when the alarm is raised. + * You can also specify a range of severity levels that the alarm would need to + execute a given callback. + +Here is a line-by-line breakdown of an example alarm implementation: + +```python +ab = AlarmBroadcaster("test_alarm") +al = AlarmListener("test_alarm") +ab.clear_alarm() +rospy.sleep(0.1) +``` +This is how you would initialize the alarm broadcaster and listener. Here +make sure to clear any previous alarm data in the broadcaster. + +```python +al.add_callback(cb1) +``` +Make sure to establish the callback function that should be executed once +the alarm is activated. + +```python +ab.raise_alarm() +rospy.sleep(0.1) +assert al.is_raised() +assert cb1_ran +``` +When the alarm is sounded via the `raise_alarm()` function, the callback will be +executed automatically. + +```python +al.clear_callbacks() + +al.add_callback(cb1, severity_required=2) +al.add_callback(cb2, call_when_raised=False) + +rospy.loginfo("Severity Range Test 1") +ab.raise_alarm(severity=4) +rospy.sleep(0.1) +assert not cb1_ran +assert cb2_ran +cb2_ran = False + +rospy.loginfo("Severity Range Test 2") +ab.raise_alarm(severity=1) +rospy.sleep(0.1) +assert cb1_ran +assert not cb2_ran +cb1_ran = False +``` +Note that you can also attach some special properties to your alarm. For instance, +you can attach multiple callback functions to the alarm. You can also configure +whether the callback function should be automatically executed when the alarm is +raised or whether it should be executed manually. Finally, you can assign a +severity level to the alarm which can tell the alarm code which callback functions +should be run. + +## Applications and Context + +The applications of ROS alarms span various contexts, with one notable application +residing in the control of the submersible vehicle's thrust and killboard. The +thrust and killboard, responsible for the sub's electronic operations, is +integrally associated with ROS alarms. Upon the board's activation or deactivation +(hard or soft kill), alarms are invoked to apprise users of these changes. The +listener's callback function comes into play, ensuring that alarms are updated +in alignment with the board's current state. This process triggered each time +the board is deactivated, creates a system whereby users are continually informed +about the board's status changes – essentially manifesting a dynamic live alarm system. diff --git a/docs/software/gazebo_guide.md b/docs/software/gazebo_guide.md new file mode 100644 index 000000000..752147b10 --- /dev/null +++ b/docs/software/gazebo_guide.md @@ -0,0 +1,327 @@ +# Gazebo Guide + +This is a guide on how to use Gazebo, including launching the sub and +viewing it inside Gazebo, how Gazebo can be controlled and manipulated, how to +use Gazebo to make world files, and where you can find more info on Gazebo. + +## Brief Introduction + +Gazebo is an open-source 3D dynamic simulator, that allows us to design, model, +and test robots and their behavior in a virtual world. Similar to game engines +like Unity, Gazebo takes in scripts and models and performs physics simulations +on them. While it is similar, Gazebo offers physics simulations at a higher level +of fidelity, a suite of sensors, and interfaces for both users and programs. + +There are many versions of Gazebo, but this guide was written for **Gazebo +Classic 11** as this is the version of Gazebo currently being used at MIL. + +### Running Gazebo + +There are many ways run Gazebo. + +* Click the "Show Applications" button on Ubuntu (the apps button located in + the bottom left corner). Then search for the Gazebo Icon and press that icon + to open Gazebo. + +* You can press Alt + F2 (or Alt+Fn+F2) to bring up the "Run a Command" window. + Then type "gazebo" and press Enter to open Gazebo. + +* You can also open a terminal and type "gazebo" and it will open Gazebo. + +To launch Gazebo will all the necessary files for simulating Subjugator, +follow these steps: + +1. Open a terminal window and execute the following command. This command uses + ROS to start all the relevant ROS nodes and to load the world file for + subjugator. This also starts a Gazebo Sever. + + ```bash + roslaunch subjugator_launch gazebo.launch --screen + ``` + + :::{note} + `--screen` forces all ROS node output to the screen. It is used for debugging. + ::: + +1. Then in another terminal window run this command to start the Gazebo + graphical client, which connects to the Gazebo Sever. + + ```bash + gazebogui + ``` + +1. Then in another terminal window run this command and then press Shift-C to + unkill the sub to allow movement. + + ```bash + amonitor kill + ``` + +1. Execute the following command to start a specific mission, replacing + "StartGate2022" with the name of the desired mission: + + ```bash + mission run StartGate2022 + ``` + +## How to use Gazebo + +### User Interface + +When you launch Gazebo you will be greeted by its user interface. + +![Gazebo Interface](/images/gazebo/Interface.png) + +The Gazebo interface consists of three main sections: The **Left Panel**, the +**Scene**, and the **Right Panel**. By default the Right Panel is hidden. This +is because we do not have anything selected. To show the right panel you can +always Click and drag the bar on the right to open it. + +![Gazebo Labeled Interface](/images/gazebo/Labeled_Interface.png) + +#### Left Panel + +The Left Panel has three tabs, each with different features. You can see +these tabs at the top of the Left Panel. You can click on them to switch +between them. The tabs are: + +##### World Tab + +The World Tab displays the models that are currently in the scene. Within this +tab, you can view and modify various model parameters, like their pose (their +position and rotation). Additionally, you can expand the GUI option to adjust +the camera view angle by modifying the camera pose. + +##### Insert Tab + +The Insert Tab allows you to add new models (objects) to the Gazebo simulation. +Here, you will find a list of file paths where your models are saved. To view +the model list, click on the arrow located on the left side of each path to +expand the folder. Select the desired model and click again in the scene to +place it. + +##### Layers Tab + +The Layers tab organizes and displays different visualization groups within +the simulation. Layers can contain one or more models, and enabling or disabling +a layer will show or hide all the models within it. While not mandatory, layers +can be helpful for organizing your simulation. Note that this tab may be empty +if no layers are defined. + +To define a layer, you will need to edit a model's SDF file. To add an +object's visuals to a layer you will need to add a `` tag for information +and then a `` tag with the layer number under each `` tag. Below +is an example: + +```xml + + + 0 + + ... + +``` + +#### Scene + +The Scene is the main window where objects are animated, and you interact with +the environment. Two toolbars are available: + +##### The Upper Toolbar + +The Upper Toolbar consists of various buttons that allow you to select, move, +rotate, and scale objects. It also provides options to create simple shapes, +as well as copy and paste objects. + +![Gazebo Upper Toolbar](/images/gazebo/Upper_Toolbar.png) + +##### The Bottom Toolbar + +The Bottom Toolbar displays information about the simulation time and its +relationship to real time. It helps you track the progress of your simulation. + +![Gazebo Bottom Toolbar](/images/gazebo/Bottom_Toolbar.png) + +#### Right Panel + +The Right Panel is used to interact with the mobile parts (joints) of a +selected model. It provides controls and settings specific to manipulating +the joints of a model. + +#### Menus (File, Edit, Camera, View, Window, Help) + +Most Linux apps have menus. These menus are usually tabs (file, edit, ...) at +the top left of an application. If you don't see it move your cursor to the +top of the application window and the menus should appear. Below describes +the features of each menu that Gazebo has. + +![Gazebo Menus](/images/gazebo/Menus.png) + +#### Mouse + +It is recommended that you use a mouse when using Gazebo. Below is a diagram +showing all the mouse controls. + +![Gazebo Mouse Controls](/images/gazebo/Mouse.png) + +However, if you want to use a trackpad you can. Below are the controls for +the trackpad: + +![Gazebo Mouse Controls](/images/gazebo/TrackPad.png) + +## How to Create Models + +The structure for most models in Gazebo is that the model is a folder that +contains a .config file, .SDF file(s), and .dae or .stl file(s). The config +file contains meta information about the model. The .SDF file contains +important simulation information like model definitions, the model's +positioning, its physical properties, etc. The .dae or .stl files contain 3D +mesh information. When creating a model it's recommended that you have all +these components. + +### Model Editor + +You can use the **Model Editor** to create simple models all within Gazebo, +but for more complex models you will want to create/write your own SDF files +and .dae files. + +To enter the **Model Editor**, click on Edit in the menu bar and select Model Editor. + +The Model Editor Interface looks similar to the regular Gazebo UI with some +slight changes. The left panel and the top toolbar have been changed to +contain only buttons and features for editing and creating parts of a model. +The bottom toolbar is now hidden as the simulation is paused. + +![Gazebo Model Editor](/images/gazebo/Model_Editor.png) + +When entering the Model Editor all other models will turn white. This can make +it hard to see the model you are currently working on if you have a lot of +models in your scene. So it may be easier to open a blank Gazebo world and +create the model using the Model Editor there. Then when you exit the Model +Editor it will ask you to save the model. This will save the model as a folder +on your computer. Then you can go back to the original world and insert +this model, by going to the insert tab (note this is the regular insert tab, +not the one in the model editor) and adding that model folder's file path. + +:::{note} +When inserting a model, make sure that the file path you pick is the path to +the parent directory. This directory contains the model folder you want to +insert. Do not put the path to the model folder. Often this parent +directory will contain all the models you want to use. The file hierarchy +might look like this: where models is the parent directory and contains the +models model1 and buoys. + +``` + models/ + ├── model_1/ + │ ├── model.config + │ ├── model1.sdf + │ ├── model1.dae + │ └── ... + ├── buoys/ + │ ├── model.config + │ ├── green.sdf + │ ├── red.sdf + │ └── ... +``` + +::: + +#### Insert Tab + +The Insert Tab allows you to add new parts, including links and models, to +your model. You have two options for inserting shapes (links): + +* Simple Shapes: Click on the desired shape in the left panel and then click + again in the scene to place it. + +* Custom Shapes: You can add COLLADA (.dae), 3D Systems (.stl), Wavefront + (.obj), and W3C SVG (.svg) files as custom shapes. Create these shapes + using 3D modeling software like Blender. + +You can also insert other models into your model as nested models. These +models can be obtained from the Gazebo Model Database +(http://gazebosim.org/models/), which should be listed as one of your file +paths under Model Databases. For example, if you need a depth sensor, you can +add a depth sensor model from the database to your model. + +#### Model Tab + +The Model Tab displays the settings for the model you are creating. Here, you +can change the model's name and modify its basic parameters. Additionally, you +can add plugins to give your model functionality here as well. + +#### Placing Shapes + +Once you insert a shape, you can use the toolbar to move, rotate, and scale +it. For finer control, you can double-click the shape or right-click and +select "Open Link Inspector" to access the link inspector. In the link +inspector, you can modify the shape's position, rotation, and scale to +achieve the desired configuration. Make sure to adjust the scale in both the +Visual and Collision tabs. + +#### Adding Joints + +To constrain the motion between shapes, you can add joints. Follow these steps: + +* Click on the joint icon in the toolbar (a line connecting two points). + +* In the Joint Creation Window, select the parent and child links (shapes) + of the joint. + +* Select the type of joint you need in the Joint Types section near the top + of the window. + +* Select the joint axis. Some joints do not have an axis. + +* Align the link (shape). Use the align links section to align the parent + and the child with each other. + +#### Adding a Plugin + +To control your model, you need to create a plugin. You can do this in the +Model Tab by specifying the necessary details for the plugin. + +You can find more information on how to create your own custom plugins [here](https://classic.gazebosim.org/tutorials?tut=ros_gzplugins). + +#### Model Creation Workflow Example + +To illustrate the model creation process, let's consider creating a car model +using Blender: + +* Create .dae files for the wheels, chassis, and other parts in Blender. + +* Insert these shapes into the Model Editor. + +* Use the toolbar and link inspector to position each shape precisely. + +* Add joints between the shapes to enable motion constraints. + +* Finally, create a plugin to control the model's behavior. + +### World File + +A World in Gazebo is used to describe the collection of robots and objects, +and global parameters like the sky, ambient light, and physics properties. +A World is the entire virtual environment that you have been +working in. The World stores important information like where all the models +are, their properties, and important global properties. + +You can save the World file by selecting File and Save World As. + +:::{note} +When using roslaunch to start Gazebo, it is crucial to update the World file +if you make any changes to the simulation environment. At MIL, there is a +dedicated `worlds` folder where Gazebo World files are saved. When you update +a World file, ensure that you replace the old file in this folder. Failing +to do so will result in the continued use of the old World file when +launching Gazebo using roslaunch. +::: + +## More Info + +If you ever need more information on how any aspect of Gazebo works or how to +use ROS with Gazebo you can check out the official Gazebo Documentation [here](https://classic.gazebosim.org/tutorials). +Some of the images used in this guide are sourced from here and we are grateful +to the creators for their exceptional work, which has been instrumental in +writing this guide. diff --git a/docs/software/getting_started.md b/docs/software/getting_started.md index 6e94e1653..a40845665 100644 --- a/docs/software/getting_started.md +++ b/docs/software/getting_started.md @@ -63,8 +63,8 @@ methods. | Architecture | URL | | ------------ | --- | -| AMD64 (most Windows computers, Intel-based Mac computers) | [focal-desktop-amd64.iso](https://cdimage.ubuntu.com/focal/daily-live/current/focal-desktop-amd64.iso) | -| ARM64 (Apple Silicon Mac computers) | [focal-desktop-arm64.iso](https://cdimage.ubuntu.com/focal/daily-live/current/focal-desktop-arm64.iso) | +| AMD64 (most Windows computers, Intel-based Mac computers) | [focal-desktop-amd64.iso](https://releases.ubuntu.com/focal/ubuntu-20.04.6-desktop-amd64.iso) | +| ARM64 (Apple Silicon Mac computers) | Unfortunately, Ubuntu 20.04 no longer has an ARM-compatible ISO. While we are working on finding/building a new iso image, you may not be able to install. | The following subsections cover various installation methods. Please choose the installation option that best meets your use case. If you're not sure what the @@ -420,7 +420,7 @@ launches the Gazebo GUI - aka, the thing you will actually interact with! If all goes according to plan, you should see our robot in its own little world! ## Installing developer tools -After you have verified that your Git setup is working appopriately, take a look +After you have verified that your Git setup is working appropriately, take a look at installing some developer tools on the [Developer Tools](/software/devtools) page. diff --git a/docs/software/help.md b/docs/software/help.md index 060f8376b..f8250fa70 100644 --- a/docs/software/help.md +++ b/docs/software/help.md @@ -63,7 +63,7 @@ for examples of it being used. ## Search the internet If your problem is not MIL-specific (issue with Linux, ROS, C++, etc), -somone has most likely had the same problem and written about it on the internet. +someone has most likely had the same problem and written about it on the internet. You'll be surprised how often you can fix your issue by Googling the error. ## Search Slack diff --git a/docs/software/index.rst b/docs/software/index.rst index 61b036c7c..5b06f569e 100644 --- a/docs/software/index.rst +++ b/docs/software/index.rst @@ -13,10 +13,13 @@ Various documentation related to practices followed by the MIL Software team. help zobelisk asyncio + rqt rostest + alarms Bash Style Guide C++ Style Guide Python Style Guide + Gazebo Guide Migrating to ROS Noetic Installing NVIDIA Drivers for RTX 2080 Installing Ubuntu 18.04 on an M-series Apple computer diff --git a/docs/software/noetic_migration.md b/docs/software/noetic_migration.md index 6a79ec10a..6a93a86cd 100644 --- a/docs/software/noetic_migration.md +++ b/docs/software/noetic_migration.md @@ -45,7 +45,7 @@ before you commit. Great, the code is now pretty! :D -If you ran `python-modernize`, it will have suggested some changes to you, indcated +If you ran `python-modernize`, it will have suggested some changes to you, indicated by `+` and `-` signs. It may look something like this: ```diff diff --git a/docs/software/rqt.md b/docs/software/rqt.md new file mode 100644 index 000000000..258c43522 --- /dev/null +++ b/docs/software/rqt.md @@ -0,0 +1,437 @@ +# Implementing GUI Based Tools Using `rqt` + +## Introduction + +RQT (ROS Qt-based GUI Framework) is a powerful tool that provides a graphical +user interface for ROS (Robot Operating System) applications. It allows users +to monitor ROS topics, interact with nodes, visualize data, and more. This +document serves as a user guide for understanding and effectively using RQT. + +### Benefits of RQT + +- **Graphical Interface**: RQT provides a user-friendly graphical interface, + making it easier to visualize data, monitor topics, and interact with nodes. + This is especially helpful for users who prefer a GUI based approach over + the command line. + +- **Centralized Tool**: RQT serves as a centralized tool for various ROS tasks, + including visualization, configuration, and debugging. This reduces the need + to switch between multiple terminals and tools, streamlining the development + process. + +- **Extensibility**: RQT's plugin architecture allows users to create custom + plugins to tailor the tool to their specific application needs. This + extensibility significantly enhances RQT's capabilities and to various + robotic systems. + +- **Integration with ROS**: RQT is tightly integrated with ROS, ensuring + seamless interaction with ROS nodes, topics, and services. This integration + simplifies the process of debugging and analyzing data in real-time. + +## Installation + +Before using RQT, make sure you have ROS installed on your system. If ROS is +not yet installed, follow the official ROS installation guide for your +operating system. Once ROS is installed, you can install RQT using the package +manager: + +```bash +sudo apt-get install ros-noetic-rqt +``` + +## Getting Started + +### Launching RQT + +To launch RQT, open a terminal and run the following command: + +```bash +rqt +``` + +This command will launch the RQT Graphical User Interface, displaying a +default layout with available plugins. + +## Monitoring Information with RQT + +RQT provides various plugins to monitor information published on specific ROS +topics. The most common plugin for this purpose is the Plot plugin. + +### Subscribing to Specific Topics + +1. Open RQT using the command mentioned in the previous section. +1. Click on the "Plugins" menu in the top toolbar and select "Topics" > + "Message Publisher." +1. A new "Message Publisher" tab will appear. Click on the "+" button to + subscribe to a specific topic. +1. Choose the desired topic from the list and click "OK." +1. Now, you can monitor the information published on the selected topic in + real-time. + +### Displaying Data with Plot Plugin + +1. Open RQT using the command mentioned earlier. +1. Click on the "Plugins" menu in the top toolbar and select + "Visualization" > "Plot." +1. A new "Plot" tab will appear. Click on the "+" button to add a plot. +1. Choose the topic you want to visualize from the list, select the message + field, and click "OK." +1. The plot will start displaying the data sent over the selected topic. + +## Configuring RQT + +RQT allows users to customize its appearance and layout to suit their preferences. + +### Theme Configuration + +1. Open RQT using the command mentioned earlier. +1. Click on the "View" menu in the top toolbar and select "Settings." +1. In the "Settings" window, go to the "Appearance" tab. +1. Here, you can change the color scheme, font size, and other visual settings. + +### Layout Configuration + +1. Open RQT using the command mentioned earlier. +1. Rearrange existing plugins by clicking on their tab and dragging them to + a new position. +1. To create a new tab, right-click on any existing tab and select "Add New Tab." +1. Drag and drop desired plugins onto the new tab. +1. Save your customized layout by going to the "View" menu and selecting + "Perspectives" > "Save Perspective." + +## Writing RQT Plugins + +RQT allows users to create their custom plugins to extend its functionality. +Writing RQT plugins is essential when the available plugins do not fully +meet your application's requirements. + +### Plugin Structure + +To write an RQT plugin, you need to follow a specific directory structure and +include Python or C++ code to implement the plugin's functionality. The +essential components of an RQT plugin are: + +- **Plugin XML (`plugin.xml`)**: This file defines the plugin and its + dependencies, specifying essential information like name, description, + and which ROS packages it depends on. + +- **Python or C++ Code**: This code contains the actual implementation of the + plugin's functionality. For Python plugins, the code should extend the + `rqt_gui_py::Plugin` class, while for C++ plugins, it should extend the + `rqt_gui_cpp::Plugin` class. + +### Implementing a Basic Plugin + +In this section, we will walk through an example of implementing a basic RQT +plugin. We will create a simple plugin to display the current time published +by a ROS topic. The plugin will consist of a label that updates with the +current time whenever a new message is received. + +#### Step 1: Create the Plugin Package + +Create a new ROS package for your plugin using the `catkin_create_pkg` command: + +```bash +catkin_create_pkg my_rqt_plugin rospy rqt_gui_py +``` + +The package dependencies `rospy` and `rqt_gui_py` are required to work with +ROS and create a Python-based RQT plugin. + +#### Step 2: Implement the Plugin Code + +Next, create a Python script for your plugin. In the `src` directory of your +package, create a file named `my_rqt_plugin.py` with the following content: + +```python +#!/usr/bin/env python + +import rospy +from rqt_gui_py.plugin import Plugin +from python_qt_binding.QtWidgets import QLabel, QVBoxLayout, QWidget +from std_msgs.msg import String +import time + +class MyRqtPlugin(Plugin): + + def __init__(self, context): + super(MyRqtPlugin, self).__init__(context) + self.setObjectName('MyRqtPlugin') + + # Create the main widget and layout + self._widget = QWidget() + layout = QVBoxLayout() + self._widget.setLayout(layout) + + # Create a label to display the current time + self._label = QLabel("Current Time: N/A") + layout.addWidget(self._label) + + # Subscribe to the topic that provides the current time + rospy.Subscriber('/current_time', String, self._update_time) + + # Add the widget to the context + context.add_widget(self._widget) + + def _update_time(self, msg): + # Update the label with the received time message + self._label.setText(f"Current Time: {msg.data}") + + def shutdown_plugin(self): + # Unregister the subscriber when the plugin is shut down + rospy.Subscriber('/current_time', String, self._update_time) + + def save_settings(self, plugin_settings, instance_settings): + # Save the plugin's settings when needed + pass + + def restore_settings(self, plugin_settings, instance_settings): + # Restore the plugin's settings when needed + pass + +``` + +In this code, we create a plugin class named `MyRqtPlugin`, which inherits +from `rqt_gui_py.Plugin`. The `__init__` method sets up the GUI elements, such +as a label, and subscribes to the `/current_time` topic to receive time updates. + +#### Step 3: Add the Plugin XML File + +Create a file named `plugin.xml` in the root of your package to define the +plugin. Add the following content to the `plugin.xml`: + +```xml + + + + A custom RQT plugin to display the current time. + + + + settings + Display the current time. + + + +``` + +This XML file defines the plugin class, its description, and the icon to be +displayed in the RQT GUI. + +#### Step 4: Build the Plugin + +Build your plugin package using `catkin_make`: + +```bash +catkin_make +``` + +#### Step 5: Launch RQT and Load the Plugin + +Launch RQT using the command mentioned earlier: + +```bash +rqt +``` + +Once RQT is open, navigate to the "Plugins" menu and select "My RQT Plugin" +from the list. The plugin will be loaded, and you should see the label +displaying the current time. + +### Adding Custom Functionality + +The example plugin we implemented is a simple demonstration of how to create +an RQT plugin. Depending on your application's requirements, you can add more +sophisticated features, such as custom data visualization, interactive +controls, and integration with other ROS elements. + +You can also improve the plugin by adding error handling, additional options +for time formatting, and the ability to pause/resume the time updates. + +## Implementing a More Advanced Plugin + +In this section, we will implement a more advanced RQT plugin that visualizes +data from multiple ROS topics using a 2D plot. This plugin will allow users to +choose which topics to plot and specify the message field to display on the X +and Y axes. Let's call this plugin "ROS Data Plotter." + +### Step 1: Create the Plugin Package + +Create a new ROS package for your plugin using the `catkin_create_pkg` command: + +```bash +catkin_create_pkg ros_data_plotter rospy rqt_gui_py matplotlib +``` + +The package dependencies `rospy`, `rqt_gui_py`, and `matplotlib` are required +to work with ROS, create a Python-based RQT plugin, and plot data, respectively. + +### Step 2: Implement the Plugin Code + +Next, create a Python script for your plugin. In the `src` directory of your +package, create a file named `ros_data_plotter.py` with the following content: + +```python +#!/usr/bin/env python + +import rospy +from rqt_gui_py.plugin import Plugin +from python_qt_binding.QtWidgets import QWidget, QVBoxLayout, QComboBox +from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas +from matplotlib.figure import Figure +from std_msgs.msg import Float32 # Modify this import based on your data type + +class ROSDataPlotter(Plugin): + + def __init__(self, context): + super(ROSDataPlotter, self).__init__(context) + self.setObjectName('ROSDataPlotter') + + # Create the main widget and layout + self._widget = QWidget() + layout = QVBoxLayout() + self._widget.setLayout(layout) + + # Create a combo box to select topics and message fields + self._topic_field_selector = QComboBox() + self._topic_field_selector.currentIndexChanged.connect(self._update_plot) + layout.addWidget(self._topic_field_selector) + + # Create the matplotlib figure and plot + self._figure = Figure() + self._canvas = FigureCanvas(self._figure) + layout.addWidget(self._canvas) + + # Initialize the plot + self._update_plot() + + # Add the widget to the context + context.add_widget(self._widget) + + def _update_plot(self): + # Clear the previous plot data + self._figure.clear() + axes = self._figure.add_subplot(111) + + # Get the selected topic and field from the combo box + selected_topic_field = self._topic_field_selector.currentText() + topic, field = selected_topic_field.split(' - ') + + # Subscribe to the selected topic + rospy.Subscriber(topic, Float32, self._plot_data) # Modify this based on your data type + + def _plot_data(self, msg): + # Get the selected topic and field from the combo box + selected_topic_field = self._topic_field_selector.currentText() + topic, field = selected_topic_field.split(' - ') + + # Plot the data on the graph + # Modify this part based on your data type and plot requirements + x_data = rospy.Time.now().to_sec() # Use timestamp as X-axis + y_data = getattr(msg, field) # Use the specified field from the message as Y-axis data + axes = self._figure.add_subplot(111) + axes.plot(x_data, y_data) + + # Refresh the plot + self._canvas.draw() + + def shutdown_plugin(self): + # Unsubscribe from the topic when the plugin is shut down + self._topic_field_selector.clear() + rospy.Subscriber(topic, Float32, self._plot_data) + + def save_settings(self, plugin_settings, instance_settings): + # Save the plugin's settings when needed + pass + + def restore_settings(self, plugin_settings, instance_settings): + # Restore the plugin's settings when needed + pass + +``` + +In this code, we create a plugin class named `ROSDataPlotter`, which inherits +from `rqt_gui_py.Plugin`. The `__init__` method sets up the GUI elements, +such as a combo box to select topics and message fields, and the matplotlib +figure to plot the data. The `_update_plot` method updates the plot whenever +a new topic or message field is selected from the combo box. The `_plot_data` +method is called when new messages are received on the selected topic, and it +plots the data on the graph. + +### Step 3: Add the Plugin XML File + +Create a file named `plugin.xml` in the root of your package to define the +plugin. Add the following content to the `plugin.xml`: + +```xml + + + + An advanced RQT plugin to plot data from multiple ROS topics. + + + + settings + Plot data from selected ROS topics. + + + +``` + +This XML file defines the plugin class, its description, and the icon +to be displayed in the RQT GUI. + +### Step 4: Build the Plugin + +Build your plugin package using `catkin_make`: + +```bash +catkin_make +``` + +### Step 5: Launch RQT and Load the Plugin + +Launch RQT + + using the command mentioned earlier: + +```bash +rqt +``` + +Once RQT is open, navigate to the "Plugins" menu and select "ROS Data Plotter" +from the list. The plugin will be loaded, and you should see a combo box to +select topics and fields, and a graph to display the plotted data. + +### Adding Custom Functionality + +In this more advanced example, we created an RQT plugin that allows users to +plot data from multiple ROS topics. The example focused on plotting Float32 +messages, but you can modify the `_plot_data` method to handle different +message types or plot multiple data series on the same graph. + +You can further enhance the plugin by adding features like legends, axis +labels, custom plot styles, data smoothing, and real-time updates. +Additionally, you can provide options to save the plotted data as CSV +or images for further analysis. + +With custom plugins, you have the freedom to tailor RQT to suit your +specific application's visualization needs, making it a versatile tool +for ROS developers. + +## External Resources + +- [ROS wiki page for rqt](https://wiki.ros.org/rqt) +- [GeorgiaTech RoboJacket's rqt tutorial](https://www.youtube.com/watch?v=o90IaCRje2I) +- [Creating Custom rqt Plugins](https://github.com/ChoKasem/rqt_tut) + +## Conclusion + +Congratulations! You have now learned the basics of using RQT, including +monitoring information sent over specific topics, configuring RQT to your +preferences, writing your custom plugins, and understanding the importance +of using RQT in ROS development. RQT provides a user-friendly and powerful +graphical interface for ROS applications, making it an essential tool for +developers and researchers working with ROS-based robotic systems. By +leveraging RQT's capabilities, you can streamline your development process +and gain valuable insights into your robot's behavior. diff --git a/docs/subjugator/index.rst b/docs/subjugator/index.rst index c34f4e7fd..7d60efae4 100644 --- a/docs/subjugator/index.rst +++ b/docs/subjugator/index.rst @@ -15,6 +15,9 @@ SubjuGator 8 Simulating Enabling Cameras + PID Controller + Navigation Tube + Watercooling electrical @@ -25,5 +28,6 @@ RoboSub RoboSub + Lessons from 2023 Lessons from 2022 Lessons from 2019 diff --git a/docs/subjugator/lessons23.md b/docs/subjugator/lessons23.md new file mode 100644 index 000000000..815f04766 --- /dev/null +++ b/docs/subjugator/lessons23.md @@ -0,0 +1,13 @@ +# Lessons from RoboSub 2023 + +Paraphrasing Dan Frank, “it's clear that if we had made all these mistakes back in Gainesville, we could have been in way better shape for the competition”. + +* **Make a plan and be familiar with the sub** - We should create a solid checklist that spells out all the basic stuff we need to nail before we leave. This should cover nailing down sub moves commands, getting familiar with the missions we already have written, testing those missions in the pool, and making sure our kill system is on point and integrates well with our software. Having a game plan for finals that's solid and picking out a reliable run that can get enough points to really boost our chances is really important. Let's not leave our presentation prep for the last minute – we need to make the presentation and practice at least a week before the competition hits. We should have deadlines for all of these. + +* **Make a plan and be familiar with the sub** - We need to have a Plan B when someone can't make it or falls sick. At least 2-3 people need to be able to rock the sub operation, change and charge batteries, replace mechanical things, etc. Roles in the team need to be crystal clear during the competition. + +* **Track issues and maintenance of parts** - We should track intermitten issues; investigate them, and deal with them, before leaving. The issues showed up every now and then, but we brushed them off thinking they’ll go away. They should've been solved earlier. Also we need a proper log of when we're swapping out parts or using them. That way, we know when it's time for replacements. We only made the move of MacArtney connector switch after stumbling on a Slack conversation from 2019. Having a grip on the tether's age would've helped sort things out sooner. + +* **Easier debugging and Sub Status** - When hardware kills don't unkill after rebooting and sticking the kill wand back in, we should've cracked open those CAN messages to see what was going wrong. Having all this info on a screen or sent to our computers would've made life simpler. About lights and screens, having status symbols or messages on screens for errors or warnings from each board would make debugging easier. Same goes for side lights on the sub – they'd be useful for the electrical and softwar. + +* **Document** - Since hardware takes more effort than firmware, we could try to make it easier to write working prototype firmware to have more time for making hardware. We currently have MIL written drivers, but they have inconsistent documentation and are confusing for some members to use (especially the ones for CAN). diff --git a/docs/subjugator/navtube.rst b/docs/subjugator/navtube.rst new file mode 100644 index 000000000..caf58ad2b --- /dev/null +++ b/docs/subjugator/navtube.rst @@ -0,0 +1,97 @@ +======== +Nav Tube +======== + +One of the major components of SubjuGator 8 is the navigation (nav tube). This pressure vessel is located right below the main pressure vessel and contains all of our navigation sensors and equipment. The navigation computer located inside the vessel allows access to the data from these components through a direct ethernet connection. + +.. warning:: + + This system was originally used on SubjuGator 7 and it was ported over with little to no changes in hardware. This does mean that the system will need a major overhaul for continued use of SubjuGator 8. + +Navigation Computer +=================== + +The navigation computer consists of a Gumstix Overo Computer-on-Module that mounts to a carrier board. The carrier board contains headers for the Teledyne Dopper Velocity Logger (DVL) connector board, Analog Devices Inertial Measurement Unit (IMU), and the pressure sensor. At one point, the navigation computer was also connected to a GPS antenna. +.. warning:: + + No documentation or PCB design files exist for the navigation computer carrier board and there are no more functioning spare boards. Additionally, only the newer GumStix Overo COM boards are available to purchase (with a lead time of roughly 1 year) which may not be compatible with the current system. Be very careful when working inside of the navigation tube. + +Troubleshooting The Nav Tube Connection +--------------------------------------- + +If you cannot ping the Navigation Computer (192.168.37.61), please ensure (in the following order) that: + +* You are pinging 192.168.37.61. +* The ethernet subconn cable has been connected between both pressure vessels. +* You restart the sub at least once. +* That the proper ethernet interface is configured in /etc/netplan YAML file. You may need to check ifconfig for the 'enpXs0' interface (where X is a whole number) if you have unplugged and replugged anything in the sub recently (especially the graphics card). Make sure to test the netplan with ``sudo netplan try`` so that you don't get locked out of the sub because of a bad configuration! +* The ethernet cables are properly connected inside the main pressure vessel. +* The ethernet cables and other cables are properly connected inside the Nav tube. +* The navigation computer is receiving power AND is turned on (green AND blue light on the GumStix). Make sure to be extra careful and vigilent when opening and handling the navigation computer. + +ADIS16400/16405-BMLZ IMU & Magnetometer +=========================================== + +The `ADIS16400-BMLZ `_ / `ADIS16405-BMLZ `_ is the device responsible for tracking our position and orientation in the water. Currently, there is an ADIS16400 IMU in the sub, but it previously used an ADIS16405. + +.. warning:: + + These components are obsolete! It may be difficult to find replacement parts in the future if SubjuGator 8 will see continued use... + +Calibrating the Magnetometer +---------------------------- + +The magnetometer inside of the ADIS16400/16405 measures the strength of the magnetic field at the orientation it faces. We use this data in conjunction with the IMU's gyroscopes and accelerometers to determine the orientation of the sub in the water. The process of calibrating the magnetometer is called magnetic hardsoft compensation and essentially changes how we bias the data it gives us. + +.. note:: + + Calibrating the magnetometer must be done in a pool. + +To calibrate the magnetometer you must first collect magnetometer data (``/imu/mag_raw``) to use for the calibration script. This can be done through the following: + +.. code-block:: bash + + $ roslaunch subjugator_launch sub8.launch + +or if you prefer to not launch the entire sub: + +.. code-block:: bash + + $ roslaunch subjugator_launch nav_box.launch imu:=true + +Then, in a separate terminal window, navigate to a known directory start recording the rosbag by typing: + +.. code-block:: bash + + $ rosbag record -O .bag /imu/mag_raw + +where is a substitute for whatever name you want to give to your bag (I recommend something memorable, like Frank or Penelope). This will record the bag data in the directory that the terminal window is in. + +When you have started recording the bag, have members who are in the water rotate the sub. Only the sub should move, not the members holding onto the sub. The calibration can be done in any order, but you must complete a full roll, pitch, and yaw rotation plus have a few minutes of data with all three occurring at the same time. Once you are done collecting data, kill the recording window using ``ctrl+c``. + +Next, we must run the calibration script with our data. This script is located in ``SubjuGator/drivers/magnetic_compensation/sub8_magnetic_hardsoft_compensation/scripts``. Type the following: + +.. code-block:: bash + + $ ./generate_config + +note that this is a python script, so + +.. code-block:: bash + + $ python3 generate_config + +is also valid. + +.. note:: + + If the script fails because of the ``fit_ellipsoid`` method and the points on the first figure are colinear or nearly colinear you may not have collected thorough enough data. The alternate possibility is a malfunctioning magnetometer. + +The output of the script should be a 3x3 matrix labeled ``scale`` and a length 3 vector labeled ``shift``. These values go into the ``scale`` and ``shift`` values located inside of ``subjugator_launch/launch/subsystems/nav_box.launch``. + +After running ``cm``, you will have (hopefully) successfully calibrated the magnetometer. Make sure to test the sub after calibration to see if the new configuration values are an improvement over the old ones. + +Important Links and Datasheets +============================== + +- `ADIS16400/ADIS16405 Datasheet `_ diff --git a/docs/subjugator/pid.rst b/docs/subjugator/pid.rst new file mode 100644 index 000000000..c2feeae29 --- /dev/null +++ b/docs/subjugator/pid.rst @@ -0,0 +1,26 @@ +PID Controller +-------------- + +SubjuGator 8 uses 6 PID controllers for its 6 degrees of freedom (x, y, z, roll, pitch, yaw). The gain for these PID controllers can be adjusted in ``adaptive_controller.yaml`` found in the ``subjugator_launch/config`` directory. Since the weight of the sub shifts with each component modified, the PID controller values have to be adjusted from time to time. There are two approaches to adjust PID values in the water: + +1. Have someone with experience in tuning PID controllers swim with the sub and use the sub's response to movement commands to adjust the gains. +2. Eliminate the error in each axis by adjusting the gains and evaluating the RMS error in each axis after sending the same movement commands. + +PID Controller Tuning Tips +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +* The learning gains (``ki`` and ``kd``) provide very little input to the wrenches applied to the sub, so it is better to treat it as a PD controller to start with. You can disable the learning gains using ``use_learned: false``. +* While you are tuning the PID values, keep a history of the values you have tried for further analysis during and after testing. You can use ``scp`` for this, but it may just be easier to take a screenshot or a photo. You can also take videos of the sub from the side of the pool to reference later. + +PID Debugging Data Collection +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. code-block:: bash + + $ roslaunch subjugator_launch bag_debugging_controller.launch prefix_name:="my_prefix" + +This launch file records a bag file containing adaptive controller ``pose_error``, ``twist_error``, ``adaptation``, ``dist``, and ``drag`` data along with ``wrench``, ``trajectory``, ``odom``, ``imu/data_raw``, and ``dvl/range`` data. This data is useful in determining if the PID values are getting better or worse using simulations. The resulting bag file will be located in ``gnc/subjugator_controller/debug_bags/``. This launch file was written by Andres Pulido. + +.. warning:: + + This launch file needs the sub launch file (``sub8.launch``) running in order to function properly. diff --git a/docs/subjugator/reference.rst b/docs/subjugator/reference.rst index a328730e9..2ed8e1296 100644 --- a/docs/subjugator/reference.rst +++ b/docs/subjugator/reference.rst @@ -47,11 +47,11 @@ Services SetValve ^^^^^^^^ -.. attributetable:: sub8_actuator_board.srv.SetValveRequest +.. attributetable:: sub_actuator_board.srv.SetValveRequest -.. class:: sub8_actuator_board.srv.SetValveRequest +.. class:: sub_actuator_board.srv.SetValveRequest - The request class for the ``sub8_actuator_board/SetValve`` service. + The request class for the ``sub_actuator_board/SetValve`` service. .. attribute:: actuator @@ -65,11 +65,11 @@ SetValve :type: bool -.. attributetable:: sub8_actuator_board.srv.SetValveResponse +.. attributetable:: sub_actuator_board.srv.SetValveResponse -.. class:: sub8_actuator_board.srv.SetValveResponse +.. class:: sub_actuator_board.srv.SetValveResponse - The response class for the ``sub8_actuator_board/SetValve`` service. + The response class for the ``sub_actuator_board/SetValve`` service. .. attribute:: success @@ -83,43 +83,46 @@ SetValve :type: bool -Exceptions ----------- -.. autoclass:: sub8_actuator_board.InvalidAddressException - Actuator Board -------------- ActuatorBoard ^^^^^^^^^^^^^ -.. attributetable:: sub8_actuator_board.ActuatorBoard +.. attributetable:: sub_actuator_board.ActuatorBoard -.. autoclass:: sub8_actuator_board.ActuatorBoard +.. autoclass:: sub_actuator_board.ActuatorBoard :members: ActuatorBoardSimulation ^^^^^^^^^^^^^^^^^^^^^^^ -.. attributetable:: sub8_actuator_board.ActuatorBoardSimulation +.. attributetable:: sub_actuator_board.ActuatorBoardSimulation -.. autoclass:: sub8_actuator_board.ActuatorBoardSimulation +.. autoclass:: sub_actuator_board.ActuatorBoardSimulation :members: -CommandMessage -^^^^^^^^^^^^^^ -.. attributetable:: sub8_actuator_board.CommandMessage +ActuatorSetPacket +^^^^^^^^^^^^^^^^^ +.. attributetable:: sub_actuator_board.ActuatorSetPacket -.. autoclass:: sub8_actuator_board.CommandMessage +.. autoclass:: sub_actuator_board.ActuatorSetPacket :members: -FeedbackMessage -^^^^^^^^^^^^^^^ -.. attributetable:: sub8_actuator_board.FeedbackMessage +ActuatorPollRequestPacket +^^^^^^^^^^^^^^^^^^^^^^^^^ +.. attributetable:: sub_actuator_board.ActuatorPollRequestPacket -.. autoclass:: sub8_actuator_board.FeedbackMessage +.. autoclass:: sub_actuator_board.ActuatorPollRequestPacket :members: -Thrust and Kill Board ---------------------- +ActuatorPollResponsePacket +^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. attributetable:: sub_actuator_board.ActuatorPollResponsePacket + +.. autoclass:: sub_actuator_board.ActuatorPollResponsePacket + :members: + +Sub8 Thrust and Kill Board +-------------------------- ThrusterAndKillBoard ^^^^^^^^^^^^^^^^^^^^ @@ -163,6 +166,51 @@ Thruster .. autoclass:: sub8_thrust_and_kill_board.Thruster :members: +Sub9 Thrust and Kill Board +-------------------------- + +HeartbeatSetPacket +^^^^^^^^^^^^^^^^^^ +.. attributetable:: sub9_thrust_and_kill_board.HeartbeatSetPacket + +.. autoclass:: sub9_thrust_and_kill_board.HeartbeatSetPacket + :members: + +HeartbeatReceivePacket +^^^^^^^^^^^^^^^^^^^^^^ +.. attributetable:: sub9_thrust_and_kill_board.HeartbeatReceivePacket + +.. autoclass:: sub9_thrust_and_kill_board.HeartbeatReceivePacket + :members: + +ThrustSetPacket +^^^^^^^^^^^^^^^ +.. attributetable:: sub9_thrust_and_kill_board.ThrustSetPacket + +.. autoclass:: sub9_thrust_and_kill_board.ThrustSetPacket + :members: + +KillStatus +^^^^^^^^^^ +.. attributetable:: sub9_thrust_and_kill_board.KillStatus + +.. autoclass:: sub9_thrust_and_kill_board.KillStatus + :members: + +KillSetPacket +^^^^^^^^^^^^^ +.. attributetable:: sub9_thrust_and_kill_board.KillSetPacket + +.. autoclass:: sub9_thrust_and_kill_board.KillSetPacket + :members: + +KillReceivePacket +^^^^^^^^^^^^^^^^^ +.. attributetable:: sub9_thrust_and_kill_board.KillReceivePacket + +.. autoclass:: sub9_thrust_and_kill_board.KillReceivePacket + :members: + Object Detection ---------------- diff --git a/docs/subjugator/watercooling.rst b/docs/subjugator/watercooling.rst new file mode 100644 index 000000000..5344e68bf --- /dev/null +++ b/docs/subjugator/watercooling.rst @@ -0,0 +1,70 @@ +============ +Watercooling +============ + +SubjuGator 8's hardware requires a watercooling loop for the CPU, graphics card, and ESCs. + +Filling the Water cooling Loop +============================== + +.. note:: + + Filling the water cooling loop requires at least 2 people. + +Materials +--------- + +* Sub Shore Power Supply +* Sub battery tube cover +* Sub battery cables +* Large Funnel +* Small Funnel +* Stool or Chair +* Deionized Water w/ Biocide +* Vacuum Pump +* Extra water cooling connectors and tubing +* Water cooling reservoir +* Bucket + +.. note:: + + While deionized water with biocide is preferred for longevity, distilled water will work fine. You should not use purified water. Bonus points if the water is dyed orange :) + +No-Pump Procedure +----------------- + +These steps should be taken when there is no external pump to help with the filling process. + +.. warning:: + + Exercise caution during steps 8 and 9 as the sub will be powered on with a water hazard nearby. + +#. Move the main vessel onto the wooden platform and down to the floor. +#. Set up the environment as shown in the picture below. +#. Gravity feed the system by filling the reservoir with water and letting the bubbles come out naturally. Move on when no more bubbles come out. +#. Empty the reservoir until the water is below the "stove pipe" (metal tube inside the reservoir). +#. Blow into the tube exposed to air to force the water into the loop while extracting the air inside of the loop. Make sure to fill the reservoir until the water is below the "stove pipe" when there is an air gap inside of the water inlet tube. Move on when you cannot remove any more bubbles. +#. Attach the vacuum pump to the tube exposed to air and pump until the pressure reaches around 10 PSI. Pinch the outlet tube and break the vacuum to allow water to fill the low pressure spots. Repeat this step until at least a decent amount of water makes it past the pump. +#. With water at the ready, connect the sub to shore power and fill the reservoir with water (you do not have to worry about the "stove pipe"). +#. Wiggle the sub around while the pump is running to extract any trapped bubbles. + +Emptying the Water Cooling Loop +=============================== + +The water cooling loop should be emptied before the sub is shipped anywhere. Emptying the water cooling loop is easier than filling it up. + +.. note:: + + Emptying the water cooling loop requires at least 2 people. + +Materials +--------- + +* Vacuum pump +* Extra water cooling connectors and tubing +* Water cooling reservoir +* Bucket + + +Procedure +--------- diff --git a/docs/testingprocedures.md b/docs/testingprocedures.md index 6b8a1c244..b1a6d4dc5 100644 --- a/docs/testingprocedures.md +++ b/docs/testingprocedures.md @@ -1,7 +1,9 @@ -# Testing Procedures +# SubjuGator Testing Procedures ## Before leaving +For best results, packing should be done at least one day before a testing session. This + ### Packing list * Power Strip * Table @@ -14,30 +16,34 @@ * Laptops * 7/16 wrench * 5/32 Allen Key -* Allen Key for paintball tank +* Duct tape and scissor +* Buoyancy foams for the sub * Pliers * Flat-head screwdriver * Kill wand +* Multimeter * Goggles / Snorkeling Tube * Pool noddles * Dive fins -* O'ring grease +* O'ring grease (Molykote 55) +* Cable grease (Molykote 44) * Hot glue gun -* Zip ties +* Hot glue sticks +* Large and small zip ties * clippers -* Towels - +* Towel(s) +* [Sunscreen](https://www.youtube.com/watch?v=sTJ7AzBIJoI) +* Tent (If going to Graham Pool or Florida Pool) +* Chairs (If going to Graham Pool) ### Software/Electrical Checklist -* Update and build code -* Verify all sensors output data -* Verify thrusters spin given command -* Verify kill (soft an hard) -* Grease all electrical connectors appropriately - +* Update (`git pull`) and build (`cm`) code. +* Verify all sensors output data. +* Verify that the correct thrusters spin given the appropriate command. +* Verify kill (soft and hard). +* Grease all electrical connectors appropriately. ## At testing site - * Get wall power to powerstrip. * Setup and connect to Network Box. * Roll tether and connect it to network box. **(DO NOT USE POE)** @@ -46,11 +52,11 @@ * SSH into sub. * Start tmux, write code. * Grease O-rings with Molykote 55 every time a pressure vessel is closed. -* Make sure ALL pneumatic tubes are inserted correctly. **(DO NOT FLOOD THE VALVE BOX)** -* Make sure all holes to paintball tank are sealed correctly. **(THIS WILL ALSO RESULT IN FLOODING IF NOT DONE)** +* * **ENSURE THAT THE RED PRESSURE RELIEF CAP ON THE BATTERY TUBE HAS BEEN SCREWED IN PLACE AFTER CHANGING BATTERIES** * Person getting into the pool must do backflip. -* Deploy sub. (check for bubbles, make sure buoyancy is correct) +* Deploy sub. (check for bubbles, make sure buoyancy is correct). * Verify odometry. -What happens when the valve box isn't closed: -![What happens when the valve box isn't closed.](/flooded_valve_box.jpg) +### Troubleshooting + +- :ref:`Troubleshooting The Nav Tube Connection` diff --git a/mil_common/axros b/mil_common/axros index 1b0399935..d0f12eedb 160000 --- a/mil_common/axros +++ b/mil_common/axros @@ -1 +1 @@ -Subproject commit 1b03999351fb5a61b202ff125f493229c2a1676b +Subproject commit d0f12eedb4622f5bbc5ba4fb63ae350f27a51f4b diff --git a/mil_common/drivers/mil_passive_sonar/README.md b/mil_common/drivers/mil_passive_sonar/README.md index 572ad4069..7010a3ae0 100644 --- a/mil_common/drivers/mil_passive_sonar/README.md +++ b/mil_common/drivers/mil_passive_sonar/README.md @@ -4,7 +4,7 @@ This package includes the main algorithm to find the direction of a pinger. It a # Running: Ensure that hydrophones -> ros is running and publishing a `mil_passive_sonar/Ping` msg. -Then run `roslaunch mil_passive_sonar mil_passive_sonar.lauch` +Then run `roslaunch mil_passive_sonar mil_passive_sonar.launch` `mil_passive_sonar/FindPinger` diff --git a/mil_common/drivers/mil_passive_sonar/scripts/triggering.py b/mil_common/drivers/mil_passive_sonar/scripts/triggering.py index 897a76949..321a20411 100755 --- a/mil_common/drivers/mil_passive_sonar/scripts/triggering.py +++ b/mil_common/drivers/mil_passive_sonar/scripts/triggering.py @@ -96,7 +96,7 @@ def get_params(self): self.enabled = rospy.get_param("~enable_on_launch") # Attributes about our target frequency range - # target Frquency in Hz + # target Frequency in Hz self.target = rospy.get_param("~target_frequency") # tolerance around that frequerncy in Hz tolerance = rospy.get_param("~frequency_tolerance") @@ -121,7 +121,7 @@ def get_params(self): self.v_sound = rospy.get_param("v_sound") # Misc attributes - # minimum gradient of the max convolution wrt time to trigger a time of arivals calculation + # minimum gradient of the max convolution wrt time to trigger a time of arrivals calculation self.threshold = rospy.get_param("~threshold") self.trigger_offset = rospy.get_param("~trigger_offset") # how far after the triggering time to make upper bound of samples at triggering in sec diff --git a/mil_common/drivers/mil_pneumatic_actuator/nodes/pneumatic_actuator_node b/mil_common/drivers/mil_pneumatic_actuator/nodes/pneumatic_actuator_node index bf8261177..920fb5a92 100755 --- a/mil_common/drivers/mil_pneumatic_actuator/nodes/pneumatic_actuator_node +++ b/mil_common/drivers/mil_pneumatic_actuator/nodes/pneumatic_actuator_node @@ -37,7 +37,7 @@ class Actuator: if isinstance(config, int): return cls.from_int(config) type_str = config["type"] - pulse_time = config["pulse_time"] if "pulse_time" in config else None + pulse_time = config.get("pulse_time") open_id = config["ports"]["open_port"]["id"] open_default = config["ports"]["open_port"]["default"] if "close_port" in config["ports"]: diff --git a/mil_common/drivers/mil_usb_to_can/CMakeLists.txt b/mil_common/drivers/mil_usb_to_can/CMakeLists.txt index 98ffc3f73..dfd241d3c 100644 --- a/mil_common/drivers/mil_usb_to_can/CMakeLists.txt +++ b/mil_common/drivers/mil_usb_to_can/CMakeLists.txt @@ -9,7 +9,7 @@ catkin_package() if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) - add_rostest(test/adder_device.test) - add_rostest(test/echo_device.test) + add_rostest(test/adder_device_sub9.test) + add_rostest(test/echo_device_sub9.test) add_rostest(test/basic.test) endif() diff --git a/mil_common/drivers/mil_usb_to_can/launch/example.launch b/mil_common/drivers/mil_usb_to_can/launch/example.launch index 68e69671f..630b8bd95 100644 --- a/mil_common/drivers/mil_usb_to_can/launch/example.launch +++ b/mil_common/drivers/mil_usb_to_can/launch/example.launch @@ -3,7 +3,7 @@ - + # Path of serial device diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/__init__.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/__init__.py index 86cce2fee..e69de29bb 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/__init__.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/__init__.py @@ -1,74 +0,0 @@ -#!/usr/bin/python3 -""" -The ``mil_usb_to_can`` package implements a driver and helper class for the -USB to CAN driver board. The package provides full functionality for communication -with the board, along with helper classes to provide a better, more structured use -of data being sent to and received from the board. - -To launch the driver, start ``driver.py``, an executable Python file. This file -will spin up a driver and interface to the board. If you are starting the driver -from a launch file, you can additionally provide information for the embedded -:class:`USBtoCANBoard` class, which handles connecting to the board. This information -can be provided through local ROS parameters: - -.. code-block:: xml - - - - - - # Path of serial device (default: "/dev/tty0") - port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A800GHCF-if00-port0 - # Baudrate of device (default: 115200) - baudrate: 115200 - # The CAN device id of the board (default: 0) - can_id: 0 - # List of python device handle classes (list of device id: python class implementation) - device_handles: - "18": sub8_thrust_and_kill_board.ThrusterAndKillBoard - "83": sub8_actuator_board.ActuatorBoard - simulated_devices: - "18": sub8_thrust_and_kill_board.ThrusterAndKillBoardSimulation - "83": sub8_actuator_board.ActuatorBoardSimulation - - - - -Many of the parameters shown are used to connect the driver to the physical board. -However, the final two parameters, ``device_handles`` and ``simulated_devices`` -are used to create device handles for specific devices. These device handles can -send and receive data from the board, and many be used to do both, or one or the -other. - -As suggested in the above XML, the package implements drivers for a physical run, -as well as a simulated run. Whether the simulated drivers are used is controlled -by the global ``/is_simulation`` parameter. The physical drivers implement -:class:`CANDeviceHandle`, whereas the simulated drivers implement from -:class:`SimulatedCANDevice`. More information on writing device handles can be -found in the documentation of each type of class. -""" - -from .application_packet import ( - ApplicationPacket, - ApplicationPacketWrongIdentifierException, -) -from .board import USBtoCANBoard -from .device import CANDeviceHandle, ExampleAdderDeviceHandle, ExampleEchoDeviceHandle -from .driver import USBtoCANDriver -from .simulation import ( - ExampleSimulatedAdderDevice, - ExampleSimulatedEchoDevice, - SimulatedCANDevice, - SimulatedUSBtoCAN, -) -from .utils import ( - ChecksumException, - CommandPacket, - InvalidEndFlagException, - InvalidFlagException, - InvalidStartFlagException, - Packet, - PayloadTooLargeException, - ReceivePacket, - USB2CANException, -) diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py new file mode 100644 index 000000000..30d817879 --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/__init__.py @@ -0,0 +1,74 @@ +#!/usr/bin/python3 +""" +The ``mil_usb_to_can`` package implements a driver and helper class for the +USB to CAN driver board. The package provides full functionality for communication +with the board, along with helper classes to provide a better, more structured use +of data being sent to and received from the board. + +To launch the driver, start ``sub8_driver.py``, an executable Python file. This file +will spin up a driver and interface to the board. If you are starting the driver +from a launch file, you can additionally provide information for the embedded +:class:`USBtoCANBoard` class, which handles connecting to the board. This information +can be provided through local ROS parameters: + +.. code-block:: xml + + + + + + # Path of serial device (default: "/dev/tty0") + port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A800GHCF-if00-port0 + # Baudrate of device (default: 115200) + baudrate: 115200 + # The CAN device id of the board (default: 0) + can_id: 0 + # List of python device handle classes (list of device id: python class implementation) + device_handles: + "18": sub8_thrust_and_kill_board.ThrusterAndKillBoard + "83": sub8_actuator_board.ActuatorBoard + simulated_devices: + "18": sub8_thrust_and_kill_board.ThrusterAndKillBoardSimulation + "83": sub8_actuator_board.ActuatorBoardSimulation + + + + +Many of the parameters shown are used to connect the driver to the physical board. +However, the final two parameters, ``device_handles`` and ``simulated_devices`` +are used to create device handles for specific devices. These device handles can +send and receive data from the board, and many be used to do both, or one or the +other. + +As suggested in the above XML, the package implements drivers for a physical run, +as well as a simulated run. Whether the simulated drivers are used is controlled +by the global ``/is_simulation`` parameter. The physical drivers implement +:class:`CANDeviceHandle`, whereas the simulated drivers implement from +:class:`SimulatedCANDevice`. More information on writing device handles can be +found in the documentation of each type of class. +""" + +from .application_packet import ( + ApplicationPacket, + ApplicationPacketWrongIdentifierException, +) +from .board import USBtoCANBoard +from .device import CANDeviceHandle, ExampleAdderDeviceHandle, ExampleEchoDeviceHandle +from .simulation import ( + ExampleSimulatedAdderDevice, + ExampleSimulatedEchoDevice, + SimulatedCANDevice, + SimulatedUSBtoCAN, +) +from .sub8_driver import USBtoCANDriver +from .utils import ( + ChecksumException, + CommandPacket, + InvalidEndFlagException, + InvalidFlagException, + InvalidStartFlagException, + Packet, + PayloadTooLargeException, + ReceivePacket, + USB2CANException, +) diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/application_packet.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/application_packet.py similarity index 100% rename from mil_common/drivers/mil_usb_to_can/mil_usb_to_can/application_packet.py rename to mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/application_packet.py diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/board.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/board.py similarity index 94% rename from mil_common/drivers/mil_usb_to_can/mil_usb_to_can/board.py rename to mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/board.py index 8f532dce6..285332e32 100755 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/board.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/board.py @@ -6,8 +6,8 @@ import serial -from mil_usb_to_can.simulation import SimulatedUSBtoCAN -from mil_usb_to_can.utils import ( # causes error if relative import used - GH-731 +from .simulation import SimulatedUSBtoCAN +from .utils import ( # causes error if relative import used - GH-731 CommandPacket, ReceivePacket, ) diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/device.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/device.py similarity index 100% rename from mil_common/drivers/mil_usb_to_can/mil_usb_to_can/device.py rename to mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/device.py diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/simulation.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/simulation.py similarity index 100% rename from mil_common/drivers/mil_usb_to_can/mil_usb_to_can/simulation.py rename to mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/simulation.py diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/sub8_driver.py similarity index 93% rename from mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py rename to mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/sub8_driver.py index ed8a92a8a..c03b318d2 100755 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/driver.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/sub8_driver.py @@ -5,10 +5,12 @@ import rospy from serial import SerialException -from mil_usb_to_can.board import ( - USBtoCANBoard, # relative import causes import error with rosrun - GH-731 +from mil_usb_to_can.sub8.board import ( + USBtoCANBoard, ) -from mil_usb_to_can.utils import USB2CANException + +# relative import causes import error with rosrun - GH-731 +from mil_usb_to_can.sub8.utils import USB2CANException if TYPE_CHECKING: from .device import CANDeviceHandle @@ -83,9 +85,7 @@ def read_packet(self) -> bool: self.handles[packet.device].on_data(packet.data) else: rospy.logwarn( - "Message received for device {}, but no handle registered".format( - packet.device, - ), + f"Message received for device {packet.device}, but no handle registered", ) return True diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/utils.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/utils.py similarity index 97% rename from mil_common/drivers/mil_usb_to_can/mil_usb_to_can/utils.py rename to mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/utils.py index 2d2558f57..7524e6dfe 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/utils.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/utils.py @@ -26,10 +26,7 @@ class ChecksumException(USB2CANException): def __init__(self, calculated, expected): super().__init__( - "Checksum was calculated as {} but reported as {}".format( - calculated, - expected, - ), + f"Checksum was calculated as {calculated} but reported as {expected}", ) @@ -117,13 +114,11 @@ def __bytes__(self) -> bytes: @overload @classmethod - def _unpack_payload(cls, data: Literal[b""]) -> None: - ... + def _unpack_payload(cls, data: Literal[b""]) -> None: ... @overload @classmethod - def _unpack_payload(cls, data: bytes) -> bytes: - ... + def _unpack_payload(cls, data: bytes) -> bytes: ... @classmethod def _unpack_payload(cls, data: bytes) -> bytes | None: @@ -458,13 +453,11 @@ def calculate_checksum(self, data: bytes) -> int: @overload @classmethod - def from_bytes(cls, data: Literal[b""]) -> None: - ... + def from_bytes(cls, data: Literal[b""]) -> None: ... @overload @classmethod - def from_bytes(cls: type[T], data: bytes) -> T: - ... + def from_bytes(cls: type[T], data: bytes) -> T: ... @classmethod def from_bytes(cls: type[T], data: bytes) -> T | None: diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/__init__.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/__init__.py new file mode 100644 index 000000000..2501f364e --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/__init__.py @@ -0,0 +1,2 @@ +from .device import CANDeviceHandle, SimulatedCANDeviceHandle +from .packet import AckPacket, NackPacket, Packet diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/device.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/device.py new file mode 100644 index 000000000..1e7638e75 --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/device.py @@ -0,0 +1,87 @@ +from __future__ import annotations + +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from .packet import Packet + from .sub9_driver import SimulatedUSBtoCANStream, USBtoCANDriver + + +class SimulatedCANDeviceHandle: + """ + Simulates a CAN device, with functions to be overridden to handle data requests + and sends from motherboard. + + Child classes can inherit from this class to implement a simulated CAN device. + + Attributes: + inbound_packets (type[Packet]): The types of packets listened to by this device. + Packets of this type will be routed to the :meth:`~.on_data` method of + the device handle. + """ + + def __init__( + self, + sim_board: SimulatedUSBtoCANStream, + inbound_packets: list[type[Packet]], + ): + self._sim_board = sim_board + self.inbound_packets = inbound_packets + + def send_data(self, data: bytes): + """ + Sends data over the serial connection. + """ + self._sim_board.send_to_bus(data) + + def on_data(self, packet: Packet): + """ + Called when an relevant incoming packet is received over the serial + connection. Relevant packets are those listed in :attr:`~.inbound_packets`. + + Partial data (ie, incomplete packets) are not sent through this event. + + Args: + packet (Packet): The incoming packet. + """ + del packet + + +class CANDeviceHandle: + """ + Base class to allow developers to write handles for communication with a + particular CAN device. The two methods of the handle allow the handle to listen + to incoming data, as well as send data. + """ + + def __init__(self, driver: USBtoCANDriver): + """ + Args: + driver (USBtoCANBoard): Driver that is used to communicate with the board. + device_id (int): The CAN ID of the device this class will handle. Not currently used. + """ + self._driver = driver + + def on_data(self, data: Packet): + """ + Called when a return packet is sent over the serial connection. In the + USB to CAN protocol, it is assumed that packets will be returned to the + motherboard in the order they are sent out. Therefore, the type of packet + sent through this event is not guaranteed, and is only determined by the + message and subclass ID sent by the individual board. + + Partial data (ie, incomplete packets) are not sent through this event. + + Args: + packet (Packet): The incoming packet. + """ + del data + + def send_data(self, data: Packet): + """ + Sends a packet over the serial connection. + + Args: + data (Packet): The packet to send. + """ + return self._driver.send_data(self, data) diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/example.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/example.py new file mode 100644 index 000000000..d0467b41d --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/example.py @@ -0,0 +1,148 @@ +import random +import string +from dataclasses import dataclass + +import rospy +from rospy_tutorials.srv import AddTwoInts +from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse + +from .device import CANDeviceHandle, SimulatedCANDeviceHandle +from .packet import Packet + + +@dataclass +class ExampleEchoRequestPacket( + Packet, + msg_id=0x99, + subclass_id=0x00, + payload_format="10s", +): + my_special_string: bytes + + +@dataclass +class ExampleEchoResponsePacket( + Packet, + msg_id=0x99, + subclass_id=0x01, + payload_format="10s", +): + my_special_string: bytes + + +@dataclass +class ExampleAdderRequestPacket( + Packet, + msg_id=0x99, + subclass_id=0x02, + payload_format="BB", +): + num_one: int + num_two: int + + +@dataclass +class ExampleAdderResponsePacket( + Packet, + msg_id=0x99, + subclass_id=0x03, + payload_format="B", +): + response: int + + +class ExampleEchoDeviceHandle(CANDeviceHandle): + """ + An example implementation of a CANDeviceHandle which will handle + a device that echos back any data sent to it. + """ + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.last_sent = None + self.count = 0 + self._srv = rospy.Service("start_echo", Trigger, self.srv_req) + + def srv_req(self, req: TriggerRequest): + while self.count < 10: + if not self.send_new_string(10): + return TriggerResponse(False, "Unable to send string of length ten.") + return TriggerResponse(True, "Complete!") + + def on_data(self, data: ExampleEchoRequestPacket): + response = data.my_special_string.decode() + if self.last_sent is None: + raise RuntimeError(f"Received {data} but have not yet sent anything") + elif response != self.last_sent[0]: + raise ValueError( + f"ERROR! Received {response} but last sent {self.last_sent}", + ) + else: + self.count += 1 + + def send_new_string(self, length: int = 10): + # Example string to test with + test = "".join([random.choice(string.ascii_letters) for _ in range(length)]) + self.last_sent = (test, rospy.Time.now()) + self.send_data(ExampleEchoRequestPacket(test.encode())) + start = rospy.Time.now() + count_now = self.count + while self.count == count_now: + if rospy.Time.now() - start > rospy.Duration(1): + return False + return True + + +class ExampleAdderDeviceHandle(CANDeviceHandle): + """ + An example implementation of a CANDeviceHandle which will handle + a device that echos back any data sent to it. + """ + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.response_received = None + self._srv = rospy.Service("add_two_ints", AddTwoInts, self.on_service_req) + + def on_service_req(self, req): + self.response_received = None + self.send_data(ExampleAdderRequestPacket(req.a, req.b)) + start = rospy.Time.now() + while self.response_received is None: + if rospy.Time.now() - start > rospy.Duration(1): + return -1 + return self.response_received.response + + def on_data(self, data: ExampleAdderResponsePacket): + self.response_received = data + + +class ExampleSimulatedEchoDevice(SimulatedCANDeviceHandle): + """ + Example implementation of a SimulatedCANDevice. + On sends, stores the transmitted data in a buffer. + When data is requested, it echos this data back. + """ + + def __init__(self, handle, inbound_packets): + # Call parent classes constructor + super().__init__(handle, inbound_packets) + + def on_data(self, data: ExampleEchoRequestPacket): + # Echo data received back onto the bus + self.send_data(bytes(ExampleEchoResponsePacket(data.my_special_string))) + + +class ExampleSimulatedAdderDevice(SimulatedCANDeviceHandle): + """ + Example implementation of a SimulatedCANDevice. + On sends, stores the transmitted data in a buffer. + When data is requested, it echos this data back. + """ + + def __init__(self, handle, inbound_packets): + # Call parent classes constructor + super().__init__(handle, inbound_packets) + + def on_data(self, data: ExampleAdderRequestPacket): + self.send_data(bytes(ExampleAdderResponsePacket(data.num_one + data.num_two))) diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py new file mode 100644 index 000000000..06f625b9b --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py @@ -0,0 +1,170 @@ +from __future__ import annotations + +import struct +from dataclasses import dataclass +from enum import Enum +from functools import lru_cache +from typing import ClassVar, get_type_hints + +SYNC_CHAR_1 = ord("3") +SYNC_CHAR_2 = ord("7") + +_packet_registry: dict[int, dict[int, type[Packet]]] = {} + + +def hexify(data: bytes) -> str: + return ":".join(f"{c:02x}" for c in data) + + +@lru_cache(maxsize=None) +def get_cache_hints(cls): + return get_type_hints(cls) + + +class ChecksumException(OSError): + def __init__( + self, + packet: type[Packet], + received: tuple[int, int], + expected: tuple[int, int], + ): + super().__init__( + f"Invalid checksum in packet of type {packet.__qualname__}: received {received}, expected {expected}", + ) + + +@dataclass +class Packet: + """ + Represents one packet sent or received by a device handle communicating to/from + the USB to CAN board. This class is able to handle packaging unique data + values into a :class:`bytes` object for sending over a data stream. + + This class should be overridden to implement unique packet payloads. Note + that this class supports three subclass arguments to assign unique message IDs, + subclass IDs, and payload formats. Note that all subclasses must be decorated + with :meth:`dataclasses.dataclass`. + + If any class members are annotated with a subclass of :class:`enum.Enum`, + the class will always make an attempt to convert the raw data value to an + instance of the enum before constructing the rest of the values in the class. + + .. code-block:: python + + from dataclasses import dataclass + + @dataclass + class ExamplePacket(Packet, msg_id = 0x02, subclass_id = 0x01, payload_format = "BHHf"): + example_char: int + example_short: int + example_short_two: int + example_float: float + + .. container:: operations + + .. describe:: bytes(x) + + Returns a :class:`bytes` object representing the data of the packet + in the specified packet format. + + Arguments: + msg_id (int): The message ID. Can be between 0 and 255. + subclass_id (int): The message subclass ID. Can be between 0 and 255. + payload_format (str): The format for the payload. This determines how + the individual payload is assembled. Each character in the format + string represents the position of one class variable. The class variables + are assembled in the order they are defined in. + """ + + msg_id: ClassVar[int] + subclass_id: ClassVar[int] + payload_format: ClassVar[str] + + def __init_subclass__(cls, msg_id: int, subclass_id: int, payload_format: str = ""): + cls.msg_id = msg_id + cls.subclass_id = subclass_id + cls.payload_format = payload_format + packets = [p for mid in _packet_registry.values() for p in mid.values()] + for packet in packets: + if packet.msg_id == msg_id and packet.subclass_id == subclass_id: + raise ValueError( + f"Cannot reuse msg_id 0x{msg_id:0x} and subclass_id 0x{subclass_id}, already used by {packet.__qualname__}", + ) + _packet_registry.setdefault(msg_id, {})[subclass_id] = cls + + def __post_init__(self): + for name, field_type in get_cache_hints(self.__class__).items(): + if ( + name + not in [ + "msg_id", + "subclass_id", + "payload_format", + ] + and not isinstance(self.__dict__[name], field_type) + and issubclass(field_type, Enum) + ): + setattr(self, name, field_type(self.__dict__[name])) + + @classmethod + def _calculate_checksum(cls, data: bytes) -> tuple[int, int]: + sum1, sum2 = 0, 0 + for byte in data: + sum1 = (sum1 + byte) % 255 + sum2 = (sum2 + sum1) % 255 + return sum1, sum2 + + def __bytes__(self): + payload = struct.pack(self.payload_format, *self.__dict__.values()) + data = struct.pack( + f"=BBBBH{len(payload)}s", + SYNC_CHAR_1, + SYNC_CHAR_2, + self.msg_id, + self.subclass_id, + len(payload), + payload, + ) + checksum = self._calculate_checksum(data[2:]) + return data + struct.pack("=BB", *checksum) + + @classmethod + def from_bytes(cls, packed: bytes) -> Packet: + """ + Constructs a packet from a packed packet in a :class:`bytes` object. + If a packet is found with the corresponding message and subclass ID, + then an instance of that packet class will be returned, else :class:`Packet` + will be returned. + """ + msg_id = packed[2] + subclass_id = packed[3] + if msg_id in _packet_registry and subclass_id in _packet_registry[msg_id]: + subclass = _packet_registry[msg_id][subclass_id] + payload = packed[6:-2] + if struct.unpack("=BB", packed[-2:]) != cls._calculate_checksum( + packed[2:-2], + ): + raise ChecksumException( + subclass, + struct.unpack("=BB", packed[-2:]), + cls._calculate_checksum(packed[2:-2]), + ) + unpacked = struct.unpack(subclass.payload_format, payload) + return subclass(*unpacked) + raise LookupError( + f"Attempted to reconstruct packet with msg_id 0x{msg_id:02x} and subclass_id 0x{subclass_id:02x}, but no packet with IDs was found.", + ) + + +@dataclass +class AckPacket(Packet, msg_id=0x00, subclass_id=0x01, payload_format=""): + """ + Common acknowledgment packet. Should only be found in response operations. + """ + + +@dataclass +class NackPacket(Packet, msg_id=0x00, subclass_id=0x00, payload_format=""): + """ + Common not-acknowledged packet. Should only be found in response operations. + """ diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/sub9_driver.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/sub9_driver.py new file mode 100755 index 000000000..90928ff8d --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/sub9_driver.py @@ -0,0 +1,275 @@ +#!/usr/bin/python3 +from __future__ import annotations + +import importlib +from collections import deque +from threading import Lock +from typing import TYPE_CHECKING, Literal, overload + +import rospy +import serial +from mil_misc_tools.serial_tools import SimulatedSerial +from serial import SerialException + +from mil_usb_to_can.sub9.device import CANDeviceHandle, SimulatedCANDeviceHandle +from mil_usb_to_can.sub9.packet import SYNC_CHAR_1, Packet + +if TYPE_CHECKING: + HandlePacketListing = tuple[ + type[SimulatedCANDeviceHandle | CANDeviceHandle], + list[Packet], + ] + + +class SimulatedUSBtoCANStream(SimulatedSerial): + """ + Simulates the USB to CAN board. Is supplied with a dictionary of simulated + CAN devices to simulate the behavior of the whole CAN network. + """ + + _devices: list[SimulatedCANDeviceHandle] + + def __init__( + self, + devices: ( + list[tuple[type[SimulatedCANDeviceHandle], list[type[Packet]]]] | None + ) = None, + ): + """ + Args: + devices (List[Tuple[Type[SimulatedCANDeviceHandle], List[Type[Packet]]]]): List of + the simulated device handles, along with the list of packets each handle + is listening for. + """ + if devices is None: + devices = [] + + self._devices = [device(self, packet_list) for device, packet_list in devices] + super().__init__() + + def send_to_bus(self, data: bytes, *, from_mobo=False): + """ + Sends data onto the simulated bus from a simulated device. + + Args: + data (bytes): The payload to send on to the bus. + from_mobo (bool): Whether the data is from the motherboard. Defaults to + False. + """ + # Deconstruct packet + p = Packet.from_bytes(data) + + if not from_mobo: + self.buffer += data + + # Send packet to appropriate handle + sent = False + for device in self._devices: + if type(p) in device.inbound_packets: + device.on_data(p) + sent = True + + if not sent and from_mobo: + rospy.logerr( + f"{sent}, {from_mobo}: Received packet of type {p.__class__.__qualname__} on simulated bus, but no one is listening for it...", + ) + + def write(self, data: bytes) -> None: + """ + Sends data to the bus. This method should only be called by the driver. + """ + self.send_to_bus(data, from_mobo=True) + + +class USBtoCANDriver: + """ + ROS Driver which implements the USB to CAN board. Allow users to specify a dictionary of + device handle classes to be loaded at runtime to handle communication with + specific devices. + """ + + _packet_deque: deque[ + SimulatedCANDeviceHandle | CANDeviceHandle + ] # Used to keep track of who gets incoming packets + _inbound_listing: dict[type[Packet], CANDeviceHandle] + + def __init__(self): + port = rospy.get_param("~port", "/dev/tty0") + baud = rospy.get_param("~baudrate", 115200) + rospy.get_param("~can_id", 0) + simulation = rospy.get_param("/is_simulation", False) + self.lock = Lock() + self._packet_deque = deque() + # If simulation mode, load simulated devices + if simulation: + rospy.logwarn( + "CAN2USB driver in simulation! Will not talk to real hardware.", + ) + devices = self.read_devices(simulated=True) + self.stream = SimulatedUSBtoCANStream(devices=devices) + else: + self.stream = serial.Serial(port=port, baudrate=baud, timeout=0.1) + + self.stream.reset_output_buffer() + self.stream.reset_input_buffer() + + self.handles = [] + self._inbound_listing = {} + for device in self.read_devices(simulated=False): + handle = device[0](self) + self.handles.append(handle) + for packet in device[1]: + self._inbound_listing[packet] = handle + + self.timer = rospy.Timer(rospy.Duration(1.0 / 20.0), self.process_in_buffer) + + def read_from_stream(self) -> bytes | None: + # Read until SOF is encourntered in case buffer contains the end of a previous packet + sof = None + for _ in range(10): + sof = self.stream.read(1) + if not len(sof): + continue + sof_int = int.from_bytes(sof, byteorder="big") + if sof_int == SYNC_CHAR_1: + break + if not isinstance(sof, bytes): + raise TimeoutError("No SOF received in one second.") + sof_int = int.from_bytes(sof, byteorder="big") + if sof_int != SYNC_CHAR_1: + print("Where da start char at?") + data = sof + # Read sync char 2, msg ID, subclass ID + data += self.stream.read(3) + length = self.stream.read(2) # read payload length + data += length + data += self.stream.read( + int.from_bytes(length, byteorder="little") + 2, + ) # read data and checksum + return data + + def read_packet(self) -> bool: + """ + Attempt to read a packet from the board. If the packet has an appropriate device + handler, then the packet is passed to the ``on_data`` method of that handler. + + Returns: + bool: The success in reading a packet. + """ + try: + with self.lock: + if self.stream.in_waiting == 0: + return False + packed_packet = self.read_from_stream() + assert isinstance(packed_packet, bytes) + # rospy.logerr(f"raw: {hexify(packed_packet)}") + packet = Packet.from_bytes(packed_packet) + except SerialException as e: + rospy.logerr(f"Error reading packet: {e}") + return False + if packet is None: + return False + if packet.__class__ in self._inbound_listing: + self._inbound_listing[packet.__class__].on_data(packet) + elif not self._packet_deque: + rospy.logwarn( + f"Message of type {packet.__class__.__qualname__} received, but no device ready to accept", + ) + else: + self._packet_deque.popleft().on_data(packet) + return True + + def process_in_buffer(self, *args) -> None: + """ + Read all available packets in the board's in-buffer. + """ + while True: + try: + self.read_packet() + except Exception as e: + rospy.logerr(f"Error when reading packets: {e}") + + def send_data( + self, + handle: CANDeviceHandle | SimulatedCANDeviceHandle, + packet: Packet, + ) -> Exception | None: + """ + Sends data using the :meth:`USBtoCANBoard.send_data` method. + + Returns: + Optional[Exception]: If data was sent successfully, nothing is returned. + Otherwise, the exception that was raised in sending is returned. + """ + try: + with self.lock: + self.stream.write(bytes(packet)) + self._packet_deque.append(handle) + return None + except SerialException as e: + rospy.logerr(f"Error writing packet: {e}") + return e + + @overload + def read_devices( + self, + *, + simulated: Literal[True], + ) -> list[tuple[type[SimulatedCANDeviceHandle], list[type[Packet]]]]: ... + + @overload + def read_devices( + self, + *, + simulated: Literal[False], + ) -> list[tuple[type[CANDeviceHandle], list[type[Packet]]]]: ... + + def read_devices( + self, + *, + simulated: bool, + ) -> list[ + tuple[ + type[SimulatedCANDeviceHandle] | type[CANDeviceHandle], + list[type[Packet]], + ], + ]: + """ + Generator to load classes from module strings specified in a dictionary. + Imports all found classes. + + Yields: + Generator[Tuple[int, Any], None, None]: Yields tuples containing the device + ID and the associated class. + """ + d = {} + res: list[ + tuple[type[SimulatedCANDeviceHandle | CANDeviceHandle], list[type[Packet]]] + ] = [] + if simulated: + d = rospy.get_param("~simulated_devices") + else: + d = rospy.get_param("~device_handles") + + for module_name, packet_list in d.items(): + # Split module from class name, import module, and get class from module + module_name, cls = module_name.rsplit(".", 1) + module = importlib.import_module(module_name) + imported_class = getattr(module, cls) + if simulated: + assert issubclass(imported_class, SimulatedCANDeviceHandle) + else: + assert issubclass(imported_class, CANDeviceHandle) + packets: list[type[Packet]] = [] + for packet in packet_list: + module_name, cls = packet.rsplit(".", 1) + module = importlib.import_module(module_name) + packets.append(getattr(module, cls)) + res.append((imported_class, packets)) + return res + + +if __name__ == "__main__": + rospy.init_node("usb_to_can_driver") + driver = USBtoCANDriver() + rospy.spin() diff --git a/mil_common/drivers/mil_usb_to_can/test/adder_device.test b/mil_common/drivers/mil_usb_to_can/test/adder_device_sub8.test similarity index 53% rename from mil_common/drivers/mil_usb_to_can/test/adder_device.test rename to mil_common/drivers/mil_usb_to_can/test/adder_device_sub8.test index 54bcafd37..5dc44b94b 100644 --- a/mil_common/drivers/mil_usb_to_can/test/adder_device.test +++ b/mil_common/drivers/mil_usb_to_can/test/adder_device_sub8.test @@ -2,14 +2,14 @@ - + device_handles: - "8": mil_usb_to_can.ExampleAdderDeviceHandle + "8": mil_usb_to_can.sub8.ExampleAdderDeviceHandle simulated_devices: - "8": mil_usb_to_can.ExampleSimulatedAdderDevice + "8": mil_usb_to_can.sub8.ExampleSimulatedAdderDevice - + diff --git a/mil_common/drivers/mil_usb_to_can/test/adder_device_sub9.test b/mil_common/drivers/mil_usb_to_can/test/adder_device_sub9.test new file mode 100644 index 000000000..6f8abe6ff --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/test/adder_device_sub9.test @@ -0,0 +1,15 @@ + + + + + + + device_handles: + mil_usb_to_can.sub9.example.ExampleAdderDeviceHandle: [mil_usb_to_can.sub9.example.ExampleAdderResponsePacket] + simulated_devices: + mil_usb_to_can.sub9.example.ExampleSimulatedAdderDevice: [mil_usb_to_can.sub9.example.ExampleAdderRequestPacket] + + + + + diff --git a/mil_common/drivers/mil_usb_to_can/test/adder_device_test.py b/mil_common/drivers/mil_usb_to_can/test/adder_device_test_sub8.py similarity index 87% rename from mil_common/drivers/mil_usb_to_can/test/adder_device_test.py rename to mil_common/drivers/mil_usb_to_can/test/adder_device_test_sub8.py index 798bf84fb..bc1486086 100755 --- a/mil_common/drivers/mil_usb_to_can/test/adder_device_test.py +++ b/mil_common/drivers/mil_usb_to_can/test/adder_device_test_sub8.py @@ -33,8 +33,8 @@ def test_2service_works(self): if __name__ == "__main__": - rospy.init_node("adder_device_test", anonymous=True) + rospy.init_node("adder_device_test_sub8", anonymous=True) import rostest - rostest.rosrun("mil_usb_to_can", "adder_device_test", AdderDeviceTest) + rostest.rosrun("mil_usb_to_can", "adder_device_test_sub8", AdderDeviceTest) unittest.main() diff --git a/mil_common/drivers/mil_usb_to_can/test/adder_device_test_sub9.py b/mil_common/drivers/mil_usb_to_can/test/adder_device_test_sub9.py new file mode 100755 index 000000000..7d55a15b8 --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/test/adder_device_test_sub9.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python3 +import unittest + +import rospy +from rospy_tutorials.srv import AddTwoInts, AddTwoIntsRequest + + +class AdderDeviceTestSub9(unittest.TestCase): + """ + Integration test for CAN2USB board driver. Talks + to a simulated CAN device which should add two integers + """ + + def __init__(self, *args): + self.srv = rospy.ServiceProxy("add_two_ints", AddTwoInts) + super().__init__(*args) + + def test_1service_exists(self): + """ + Test raising/clearing kill alarm (user kill) will cause same change in hw-kill + """ + try: + self.srv.wait_for_service(5) + except rospy.ServiceException as e: + self.fail(f"Service error: {e}") + + def test_2service_works(self): + a = 3 + b = 6 + correct_sum = a + b + res = self.srv(AddTwoIntsRequest(a=a, b=b)) + self.assertEqual(res.sum, correct_sum) + + +if __name__ == "__main__": + rospy.init_node("adder_device_test_sub9", anonymous=True) + import rostest + + rostest.rosrun("mil_usb_to_can", "adder_device_test_sub9", AdderDeviceTestSub9) + unittest.main() diff --git a/mil_common/drivers/mil_usb_to_can/test/basic.test b/mil_common/drivers/mil_usb_to_can/test/basic.test index 02b7e1969..ef4b8100b 100644 --- a/mil_common/drivers/mil_usb_to_can/test/basic.test +++ b/mil_common/drivers/mil_usb_to_can/test/basic.test @@ -1,3 +1,4 @@ + diff --git a/mil_common/drivers/mil_usb_to_can/test/echo_device.test b/mil_common/drivers/mil_usb_to_can/test/echo_device_sub8.test similarity index 52% rename from mil_common/drivers/mil_usb_to_can/test/echo_device.test rename to mil_common/drivers/mil_usb_to_can/test/echo_device_sub8.test index ae64c9b54..f5c2ca2c9 100644 --- a/mil_common/drivers/mil_usb_to_can/test/echo_device.test +++ b/mil_common/drivers/mil_usb_to_can/test/echo_device_sub8.test @@ -2,14 +2,14 @@ - + device_handles: - "8": mil_usb_to_can.ExampleEchoDeviceHandle + "8": mil_usb_to_can.sub8.ExampleEchoDeviceHandle simulated_devices: - "8": mil_usb_to_can.ExampleSimulatedEchoDevice + "8": mil_usb_to_can.sub8.ExampleSimulatedEchoDevice - + diff --git a/mil_common/drivers/mil_usb_to_can/test/echo_device_sub9.test b/mil_common/drivers/mil_usb_to_can/test/echo_device_sub9.test new file mode 100644 index 000000000..64d79fd44 --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/test/echo_device_sub9.test @@ -0,0 +1,15 @@ + + + + + + + device_handles: + mil_usb_to_can.sub9.example.ExampleEchoDeviceHandle: [mil_usb_to_can.sub9.example.ExampleEchoResponsePacket] + simulated_devices: + mil_usb_to_can.sub9.example.ExampleSimulatedEchoDevice: [mil_usb_to_can.sub9.example.ExampleEchoRequestPacket] + + + + + diff --git a/mil_common/drivers/mil_usb_to_can/test/echo_device_test.py b/mil_common/drivers/mil_usb_to_can/test/echo_device_test_sub8.py similarity index 83% rename from mil_common/drivers/mil_usb_to_can/test/echo_device_test.py rename to mil_common/drivers/mil_usb_to_can/test/echo_device_test_sub8.py index 48a5c7a31..cbf698b3a 100755 --- a/mil_common/drivers/mil_usb_to_can/test/echo_device_test.py +++ b/mil_common/drivers/mil_usb_to_can/test/echo_device_test_sub8.py @@ -24,8 +24,8 @@ def test_correct_response(self): if __name__ == "__main__": - rospy.init_node("echo_device_test", anonymous=True) + rospy.init_node("echo_device_test_sub9", anonymous=True) import rostest - rostest.rosrun("mil_usb_to_can", "echo_device_test", EchoDeviceTest) + rostest.rosrun("mil_usb_to_can", "echo_device_test_sub9", EchoDeviceTest) unittest.main() diff --git a/mil_common/drivers/mil_usb_to_can/test/echo_device_test_sub9.py b/mil_common/drivers/mil_usb_to_can/test/echo_device_test_sub9.py new file mode 100755 index 000000000..d99ccd236 --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/test/echo_device_test_sub9.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python3 +import unittest + +import rospy +from std_srvs.srv import Trigger, TriggerRequest + + +class EchoDeviceTestSub9(unittest.TestCase): + """ + Integration test for CAN2USB board driver. Talks + to a simulated CAN device which should add two integers + """ + + def __init__(self, *args): + self.srv = rospy.ServiceProxy("start_echo", Trigger) + super().__init__(*args) + + def test_correct_response(self): + """ + Test we can get correct data through CAN bus at least ten times. + """ + self.srv.wait_for_service(1) + self.assertTrue(self.srv(TriggerRequest())) + + +if __name__ == "__main__": + rospy.init_node("echo_device_test_sub9", anonymous=True) + import rostest + + rostest.rosrun("mil_usb_to_can", "echo_device_test_sub9", EchoDeviceTestSub9) + unittest.main() diff --git a/mil_common/drivers/mil_usb_to_can/test/test_application_packets.py b/mil_common/drivers/mil_usb_to_can/test/test_application_packets.py index 82a428bbf..e4e4c90ae 100755 --- a/mil_common/drivers/mil_usb_to_can/test/test_application_packets.py +++ b/mil_common/drivers/mil_usb_to_can/test/test_application_packets.py @@ -5,7 +5,7 @@ import unittest import rostest -from mil_usb_to_can import ApplicationPacket, CommandPacket +from mil_usb_to_can.sub8 import ApplicationPacket, CommandPacket class BasicApplicationPacketTest(unittest.IsolatedAsyncioTestCase): diff --git a/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py b/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py new file mode 100755 index 000000000..c58a8c32c --- /dev/null +++ b/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py @@ -0,0 +1,63 @@ +#! /usr/bin/env python3 +import unittest +from dataclasses import dataclass + +import rostest +from mil_usb_to_can.sub9 import Packet +from mil_usb_to_can.sub9.packet import SYNC_CHAR_1, SYNC_CHAR_2 + + +@dataclass +class TestPacket(Packet, msg_id=0x47, subclass_id=0x44, payload_format="?Hd"): + example_bool: bool + example_int: int + example_float: float + + +class BasicApplicationPacketTest(unittest.IsolatedAsyncioTestCase): + """ + Tests basic application packt functionality. + """ + + def test_simple_packet(self): + packet = TestPacket(False, 42, 3.14) + self.assertEqual(packet.msg_id, 0x47) + self.assertEqual(packet.subclass_id, 0x44) + self.assertEqual(packet.payload_format, "?Hd") + self.assertEqual(packet.example_bool, False) + self.assertEqual(packet.example_int, 42) + self.assertEqual(packet.example_float, 3.14) + + def test_assembled_packet(self): + packet = TestPacket(False, 42, 3.14) + assembled = bytes(TestPacket(False, 42, 3.14)) + self.assertEqual(assembled[0], SYNC_CHAR_1) + self.assertEqual(assembled[1], SYNC_CHAR_2) + self.assertEqual(assembled[2], packet.msg_id) + self.assertEqual(assembled[3], packet.subclass_id) + + def test_format(self): + packet = TestPacket(False, 42, 3.14) + self.assertEqual( + TestPacket.from_bytes(TestPacket.__bytes__(packet)), + packet, + ) + + def test_comparisons(self): + packet = TestPacket(False, 42, 3.14) + packet_two = TestPacket(False, 42, 3.14) + self.assertEqual(packet, packet_two) + with self.assertRaises(TypeError): + packet < packet_two + with self.assertRaises(TypeError): + packet > packet_two + + +if __name__ == "__main__": + packet = TestPacket(False, 42, 3.14) + rostest.rosrun( + "mil_usb_to_can", + "test_application_packets_sub9", + BasicApplicationPacketTest, + ) + unittest.main() diff --git a/mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp b/mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp index d2ec5a6bf..6c2794453 100644 --- a/mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp +++ b/mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp @@ -4,7 +4,9 @@ #include #include +#include #include +#include namespace odometry_utils { @@ -13,11 +15,17 @@ class odometry_to_tf : public nodelet::Nodelet private: ros::Subscriber odom_sub; tf::TransformBroadcaster tf_br; + std::map _last_tf_stamps; void handle_odom(const nav_msgs::Odometry::ConstPtr& msg) { tf::Transform transform; poseMsgToTF(msg->pose.pose, transform); + if (_last_tf_stamps.count(msg->header.frame_id) && _last_tf_stamps[msg->header.frame_id] == msg->header.stamp) + { + return; + } + _last_tf_stamps[msg->header.frame_id] = msg->header.stamp; tf::StampedTransform stamped_transform(transform, msg->header.stamp, msg->header.frame_id, msg->child_frame_id); tf_br.sendTransform(stamped_transform); } diff --git a/mil_common/gnc/rawgps_common/src/rawgps_common/gps.py b/mil_common/gnc/rawgps_common/src/rawgps_common/gps.py index b6acd67db..d8fb03d5b 100644 --- a/mil_common/gnc/rawgps_common/src/rawgps_common/gps.py +++ b/mil_common/gnc/rawgps_common/src/rawgps_common/gps.py @@ -354,8 +354,7 @@ def predict_pos_deltat_r(self, t): assert abs(t_k) < week_length / 2 if not (abs(t_k) < 6 * 60 * 60): print( - "ERROR: ephemeris predicting more than 6 hours from now (%f hours)" - % (t_k / 60 / 60,), + f"ERROR: ephemeris predicting more than 6 hours from now ({t_k / 60 / 60:f} hours)", ) n = n_0 + self.Deltan M_k = self.M_0 + n * t_k @@ -650,9 +649,7 @@ def generate_satellite_message( for i in range(2): dt = t - eph.t_oc assert abs(dt) < week_length / 2 - deltat_SV_L1 = ( - eph.a_f0 + eph.a_f1 * dt + eph.a_f2 * dt**2 + deltat_r - eph.T_GD - ) + deltat_SV_L1 = eph.a_f0 + eph.a_f1 * dt + eph.a_f2 * dt**2 + deltat_r - eph.T_GD t = t_SV - deltat_SV_L1 if i == 1: sat_pos, deltat_r, sat_vel = eph.predict(t) @@ -692,14 +689,16 @@ def generate_satellite_message( time=-pseudo_range / c - deltat_SV_L1, T_iono=T_iono, T_tropo=T_tropo, - carrier_distance=carrier_cycles * c / L1_f0 - if carrier_cycles is not None - else nan, + carrier_distance=( + carrier_cycles * c / L1_f0 if carrier_cycles is not None else nan + ), doppler_velocity=doppler_freq * c / L1_f0, direction_enu=Vector3(*direction_enu), - velocity_plus_drift=doppler_freq * c / L1_f0 + direction.dot(sat_vel) - if doppler_freq is not None - else nan, + velocity_plus_drift=( + doppler_freq * c / L1_f0 + direction.dot(sat_vel) + if doppler_freq is not None + else nan + ), ) diff --git a/mil_common/mil_missions/mil_missions_core/__init__.py b/mil_common/mil_missions/mil_missions_core/__init__.py index f4244279d..e603f4026 100644 --- a/mil_common/mil_missions/mil_missions_core/__init__.py +++ b/mil_common/mil_missions/mil_missions_core/__init__.py @@ -4,6 +4,7 @@ from to create robot-specific mission tasks. These mission classes can be imported by the mission runner and then run. """ + from .base_mission import BaseMission from .chain_with_timeout import MakeChainWithTimeout from .exceptions import ( diff --git a/mil_common/mil_missions/mil_missions_core/base_mission.py b/mil_common/mil_missions/mil_missions_core/base_mission.py index bc41dc62c..e6e5e01df 100644 --- a/mil_common/mil_missions/mil_missions_core/base_mission.py +++ b/mil_common/mil_missions/mil_missions_core/base_mission.py @@ -121,6 +121,7 @@ def send_feedback(self, message: str) -> None: mission is a child mission, it will call the send_feedback_child of its parent, allowing missions to choose how to use the feedback from its children. """ + message = str(message) if self.parent: self.parent.send_feedback_child(message, self) else: diff --git a/mil_common/mil_missions/mil_missions_gui/dashboard.py b/mil_common/mil_missions/mil_missions_gui/dashboard.py index f3f224243..440dc85b9 100755 --- a/mil_common/mil_missions/mil_missions_gui/dashboard.py +++ b/mil_common/mil_missions/mil_missions_gui/dashboard.py @@ -195,11 +195,10 @@ def ui_log(self, string): """ self.lock.acquire() date_time = datetime.datetime.fromtimestamp(rospy.Time.now().to_time()) - time_str = "{}:{}:{}".format( - date_time.hour, - date_time.minute, - date_time.second, - ).ljust(12, " ") + time_str = f"{date_time.hour}:{date_time.minute}:{date_time.second}".ljust( + 12, + " ", + ) formatted = time_str + string self.feedback_list.addItem(formatted) self.lock.release() @@ -242,9 +241,7 @@ def transition_cb(self, goal, handler): self.current_mission_status = terminal_state self.current_mission_status_label.setText(self.current_mission_status) self.ui_log( - "FINISHED: mission finished ({})".format( - self.current_mission_status, - ), + f"FINISHED: mission finished ({self.current_mission_status})", ) def reload_available_missions(self, _): diff --git a/mil_common/mil_missions/nodes/mission_client b/mil_common/mil_missions/nodes/mission_client index 2ff81d944..230547f8e 100755 --- a/mil_common/mil_missions/nodes/mission_client +++ b/mil_common/mil_missions/nodes/mission_client @@ -30,9 +30,7 @@ class MissionClientCli: missions = MissionClient.available_missions() if not missions: print( - "{} param not set. Is mission runner running?".format( - MissionClient.LIST_PARAM, - ), + f"{MissionClient.LIST_PARAM} param not set. Is mission runner running?", ) return print("Available Missions:") diff --git a/mil_common/mil_missions/nodes/mission_server b/mil_common/mil_missions/nodes/mission_server index 3548d0b58..0009b8b8e 100755 --- a/mil_common/mil_missions/nodes/mission_server +++ b/mil_common/mil_missions/nodes/mission_server @@ -29,8 +29,10 @@ class MissionRunner: missions_loaded: bool base_mission: type[BaseMission] missions: dict[str, type[BaseMission]] + _running_tasks: set[asyncio.Task] def __init__(self): + self._running_tasks = set() pass async def init(self): @@ -213,6 +215,7 @@ class MissionRunner: self.mission = self.missions[goal.mission]() self.mission_future = asyncio.create_task(self.run_with_callback(parameters)) self.mission_future.add_done_callback(self.mission_finished_cb) + self._running_tasks.add(self.mission_future) async def run_with_callback(self, parameters): try: @@ -242,6 +245,7 @@ class MissionRunner: mission, or raises an exception. Publishes the correct result to the action clients. """ result = DoMissionResult() + self._running_tasks.remove(task) try: task_result = task.result() diff --git a/mil_common/mil_missions/test/test_import_missions.py b/mil_common/mil_missions/test/test_import_missions.py index e03fb7e82..c09c8f13b 100755 --- a/mil_common/mil_missions/test/test_import_missions.py +++ b/mil_common/mil_missions/test/test_import_missions.py @@ -21,10 +21,7 @@ def test_import_missions(self): if not hasattr(mission_module, self.base_class): self.fail( - msg="{} doesn't have base mission {}".format( - self.module, - self.base_class, - ), + msg=f"{self.module} doesn't have base mission {self.base_class}", ) base_mission = getattr(mission_module, self.base_class) diff --git a/mil_common/perception/mil_vision/include/mil_vision_lib/cv_tools.hpp b/mil_common/perception/mil_vision/include/mil_vision_lib/cv_tools.hpp index 4709fa85f..af6f30853 100644 --- a/mil_common/perception/mil_vision/include/mil_vision_lib/cv_tools.hpp +++ b/mil_common/perception/mil_vision/include/mil_vision_lib/cv_tools.hpp @@ -76,7 +76,7 @@ unsigned int select_hist_mode(std::vector &histogram_modes, unsigned // Takes in a grayscale image and segments out a semi-homogenous foreground // object with pixel intensities close to . Tuning of last three -// parameters may imrove results but default values should work well in +// parameters may improve results but default values should work well in // most cases. void statistical_image_segmentation(const cv::Mat &src, cv::Mat &dest, cv::Mat &debug_img, const int hist_size, const float **ranges, const int target, std::string image_name = "Unnamed Image", diff --git a/mil_common/perception/mil_vision/mil_vision_tools/color_classifier.py b/mil_common/perception/mil_vision/mil_vision_tools/color_classifier.py index df48a5881..3ff55caef 100755 --- a/mil_common/perception/mil_vision/mil_vision_tools/color_classifier.py +++ b/mil_common/perception/mil_vision/mil_vision_tools/color_classifier.py @@ -94,7 +94,7 @@ def classify_features(self, features: np.ndarray) -> int: def feature_probabilities(self, features: np.ndarray) -> List[float]: """ - Allows child classes to give probabilties for each possible class given + Allows child classes to give probabilities for each possible class given a features vector, instead of just one classification. By default, gives 1.0 to classified class and 0.0 to others. @@ -210,6 +210,8 @@ def read_from_csv(self, training_file: Optional[str] = None): Args: training_file (Optional[str]): The name of the training file. """ + if training_file is None: + raise NotImplementedError("Cannot read without any training file.") training_file = _get_param(training_file, self.training_file) df = pandas.read_csv(training_file) classes = df.values[:, 1] diff --git a/mil_common/perception/mil_vision/mil_vision_tools/cv_tools.py b/mil_common/perception/mil_vision/mil_vision_tools/cv_tools.py index dcfb694a1..42b6112b2 100644 --- a/mil_common/perception/mil_vision/mil_vision_tools/cv_tools.py +++ b/mil_common/perception/mil_vision/mil_vision_tools/cv_tools.py @@ -79,7 +79,7 @@ def __init__( self.conversion_code = conversion_code @classmethod - def from_dict(cls, d, in_space: str = "BGR", thresh_space: str = None): + def from_dict(cls, d, in_space: str = "BGR", thresh_space: Optional[str] = None): """ Loads thresholds from a dictionary. See examples for valid dictionaries. @@ -114,9 +114,7 @@ def from_dict(cls, d, in_space: str = "BGR", thresh_space: str = None): d.keys(), ), ) - assert thresh_space in d, "{} color space not in dictionary".format( - thresh_space, - ) + assert thresh_space in d, f"{thresh_space} color space not in dictionary" inner = d[thresh_space] if "low" in inner and "high" in inner: return cls( diff --git a/mil_common/perception/mil_vision/mil_vision_tools/shape_finder.py b/mil_common/perception/mil_vision/mil_vision_tools/shape_finder.py index 5ba5f78cb..33479a753 100644 --- a/mil_common/perception/mil_vision/mil_vision_tools/shape_finder.py +++ b/mil_common/perception/mil_vision/mil_vision_tools/shape_finder.py @@ -213,7 +213,7 @@ def get_pose_3D( cam (Optional[PinholeCameraModel]): The camera model. rectified (bool): If ``cam`` is set, set True if corners were found in an already rectified image (image_rect_color topic). - instrinsics (np.ndarray): Camera intrinisic matrix. + intrinsics (np.ndarray): Camera intrinisic matrix. dist_coeffs (np.ndarray): Camera distortion coefficients. Returns: @@ -304,12 +304,12 @@ def __init__(self, length=1.0, width=1.0): self.model_2D = np.zeros((50, 1, 2), dtype=np.int) # Approximate an ellipse with 50 points, so that verify_contour is reasonable fast still for idx, theta in enumerate(np.linspace(0.0, 2.0 * np.pi, num=50)): - self.model_2D[idx][0][ - 0 - ] = self.length * 0.5 * scale + self.length * 0.5 * scale * np.cos(theta) - self.model_2D[idx][0][ - 1 - ] = self.width * 0.5 * scale + self.width * 0.5 * scale * np.sin(theta) + self.model_2D[idx][0][0] = ( + self.length * 0.5 * scale + self.length * 0.5 * scale * np.cos(theta) + ) + self.model_2D[idx][0][1] = ( + self.width * 0.5 * scale + self.width * 0.5 * scale * np.sin(theta) + ) def get_corners(self, contour, debug_image=None): """ diff --git a/mil_common/perception/mil_vision/mil_vision_tools/vision_node.py b/mil_common/perception/mil_vision/mil_vision_tools/vision_node.py index d1c8d37e9..caaaf7fd6 100755 --- a/mil_common/perception/mil_vision/mil_vision_tools/vision_node.py +++ b/mil_common/perception/mil_vision/mil_vision_tools/vision_node.py @@ -27,7 +27,7 @@ def create_object_msg( is needed/available in your application. Args: - name (str): Name of the identifed object. + name (str): Name of the identified object. attributes (str): Attributes to attach to message, the purpose and value of this attribute will vary by application. Defaults to an empty string. confidence (Optional[float]): Float between 0 and 1 describing the confidence diff --git a/mil_common/perception/mil_vision/ros_tools/on_the_fly_thresholder.py b/mil_common/perception/mil_vision/ros_tools/on_the_fly_thresholder.py index 74fbc58dd..e70de289e 100755 --- a/mil_common/perception/mil_vision/ros_tools/on_the_fly_thresholder.py +++ b/mil_common/perception/mil_vision/ros_tools/on_the_fly_thresholder.py @@ -78,9 +78,11 @@ def __init__(self, img, thresh_type="bgr"): def update_mask(self): self.lower, self.upper = self.trackbars.get_bounds() self.mask = cv2.inRange( - self.image - if self.thresh_type == "bgr" - else cv2.cvtColor(self.image, cv2.COLOR_BGR2HSV), + ( + self.image + if self.thresh_type == "bgr" + else cv2.cvtColor(self.image, cv2.COLOR_BGR2HSV) + ), self.lower, self.upper, ) diff --git a/mil_common/perception/mil_vision/src/mil_vision_lib/active_contours.cpp b/mil_common/perception/mil_vision/src/mil_vision_lib/active_contours.cpp index b6b35bba6..46d902eee 100644 --- a/mil_common/perception/mil_vision/src/mil_vision_lib/active_contours.cpp +++ b/mil_common/perception/mil_vision/src/mil_vision_lib/active_contours.cpp @@ -135,7 +135,7 @@ void growRoute(const vector& partial, const vector& occupied, auto next_partial = partial; next_partial.push_back(new_elem); - auto next_tails = getHoodIdxs(new_elem, true); // true --> include border + auto next_tails = getHoodIdxs(new_elem, true); // true --> include border auto find_exit_itr = find(next_tails.begin(), next_tails.end(), exit); if (find_exit_itr != next_tails.end() && *find_exit_itr != tail) // add to cache if exit is a possible next tail { diff --git a/mil_common/perception/mil_vision/src/mil_vision_lib/colorizer/pcd_colorizer.cpp b/mil_common/perception/mil_vision/src/mil_vision_lib/colorizer/pcd_colorizer.cpp index a0bd83adb..14333e5e3 100644 --- a/mil_common/perception/mil_vision/src/mil_vision_lib/colorizer/pcd_colorizer.cpp +++ b/mil_common/perception/mil_vision/src/mil_vision_lib/colorizer/pcd_colorizer.cpp @@ -24,7 +24,7 @@ PcdColorizer::PcdColorizer(ros::NodeHandle nh, string input_pcd_topic) } catch (std::exception &e) { - _err_msg = "COLORIZER: Suscriber or publisher error caught: "_s + e.what(); + _err_msg = "COLORIZER: Subscriber or publisher error caught: "_s + e.what(); ROS_ERROR(_err_msg.c_str()); return; } diff --git a/mil_common/perception/mil_vision/src/mil_vision_lib/image_filtering.cpp b/mil_common/perception/mil_vision/src/mil_vision_lib/image_filtering.cpp index bbf89ff63..6643b6408 100644 --- a/mil_common/perception/mil_vision/src/mil_vision_lib/image_filtering.cpp +++ b/mil_common/perception/mil_vision/src/mil_vision_lib/image_filtering.cpp @@ -7,7 +7,7 @@ cv::Mat rotateKernel(const cv::Mat &kernel, float theta, bool deg, bool no_expan theta = deg ? theta : theta * mil_tools::PI / 180.0f; cv::Point2f c_org{ kernel.cols * 0.5f, kernel.rows * 0.5f }; // center of original - if (no_expand) // rotates without expanding the canvas + if (no_expand) // rotates without expanding the canvas { cv::Mat result; cv::Mat rot_mat = cv::getRotationMatrix2D(c_org, theta, 1.0f); diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/src/ogrid_manager.cpp b/mil_common/perception/point_cloud_object_detection_and_recognition/src/ogrid_manager.cpp index 0a8ed2d94..b921ae016 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/src/ogrid_manager.cpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/src/ogrid_manager.cpp @@ -104,7 +104,7 @@ void OgridManager::update_config(Config const& config) ogrid_.info.height = height_meters_ / resolution_meters_per_cell_; ogrid_.info.origin.position.x = -1. * width_meters_ / 2.; //-1. * width_meters_ * resolution_meters_per_cell_ / 2.; ogrid_.info.origin.position.y = - -1. * height_meters_ / 2.; // -1. * height_meters_ * resolution_meters_per_cell_ / 2.; + -1. * height_meters_ / 2.; // -1. * height_meters_ * resolution_meters_per_cell_ / 2.; ogrid_.info.origin.orientation.w = 1; ogrid_.data.resize(ogrid_.info.width * ogrid_.info.height); ogrid_mat_ = cv::Mat(cv::Size(ogrid_.info.width, ogrid_.info.height), CV_8UC1, ogrid_.data.data()); diff --git a/mil_common/ros_alarms/nodes/alarm_server.py b/mil_common/ros_alarms/nodes/alarm_server.py index bb602304c..b3a023778 100755 --- a/mil_common/ros_alarms/nodes/alarm_server.py +++ b/mil_common/ros_alarms/nodes/alarm_server.py @@ -178,9 +178,9 @@ def _create_alarm_handlers(self): if alarm_name in self.alarms: self.alarms[alarm_name].update(h.initial_alarm) else: - self.alarms[ - alarm_name - ] = h.initial_alarm # Update even if already added to server + self.alarms[alarm_name] = ( + h.initial_alarm + ) # Update even if already added to server elif ( alarm_name not in self.alarms ): # Add default initial if not there already diff --git a/mil_common/ros_alarms/ros_alarms/__init__.py b/mil_common/ros_alarms/ros_alarms/__init__.py index 600cc75c8..9f257faae 100644 --- a/mil_common/ros_alarms/ros_alarms/__init__.py +++ b/mil_common/ros_alarms/ros_alarms/__init__.py @@ -39,6 +39,7 @@ Topic publishing any getting/setting updates that occur on alarms. """ + from .alarms import ( Alarm, AlarmBroadcaster, diff --git a/mil_common/ros_alarms/ros_alarms/handler_template.py b/mil_common/ros_alarms/ros_alarms/handler_template.py index 966185a1d..90ae49350 100644 --- a/mil_common/ros_alarms/ros_alarms/handler_template.py +++ b/mil_common/ros_alarms/ros_alarms/handler_template.py @@ -105,4 +105,4 @@ def meta_predicate(self, meta_alarm: Alarm, alarms) -> Union[bool, Alarm]: should be raised, or a new Alarm object which the calling alarm should update itself to. """ - return any([alarm.raised for name, alarm in alarms.items()]) + return any(alarm.raised for name, alarm in alarms.items()) diff --git a/mil_common/ros_alarms/test/roscpp/heartbeat_monitor_test.cpp b/mil_common/ros_alarms/test/roscpp/heartbeat_monitor_test.cpp index 31650c0a7..15e173034 100644 --- a/mil_common/ros_alarms/test/roscpp/heartbeat_monitor_test.cpp +++ b/mil_common/ros_alarms/test/roscpp/heartbeat_monitor_test.cpp @@ -28,7 +28,7 @@ TEST(HeartbeatMonitorTest, heartbeatMonitorTest) // Publish heartbeat ros::Publisher heartbeat_pub = nh.advertise(heartbeat_topic, 1000); - auto pub_valid = [&heartbeat_pub](bool valid) { // Can publish a valid or invalid heartbeat + auto pub_valid = [&heartbeat_pub](bool valid) { // Can publish a valid or invalid heartbeat std_msgs::String msg; msg.data = (valid ? "Will pass the predicate" : ""); // second one won't heartbeat_pub.publish(msg); @@ -97,7 +97,7 @@ TEST(HeartbeatMonitorTest, heartbeatMonitorTest) while (ros::Time::now() - hb_monitor.getLastBeatTime() < time_to_clear) // Shouldn't recover here, { // invalid heartbeat sleep_until(time_to_raise * 0.8, ros::Time::now()); - pub_valid(false); // False --> publish invalid heartbeat + pub_valid(false); // False --> publish invalid heartbeat } EXPECT_TRUE(listener.queryRaised()); diff --git a/mil_common/ros_alarms/test/roscpp/listener_test.cpp b/mil_common/ros_alarms/test/roscpp/listener_test.cpp index a9367fe03..f365a1305 100644 --- a/mil_common/ros_alarms/test/roscpp/listener_test.cpp +++ b/mil_common/ros_alarms/test/roscpp/listener_test.cpp @@ -51,7 +51,7 @@ TEST(ListenerTest, listenerTest) ab.getAlarm() = pxy; ab.clear(); // alarm starts off cleared - // Last update time happened wehen we called ab.clear() + // Last update time happened when we called ab.clear() auto first_query = listener.getLastUpdateTime(); ab.updateSeverity(5); // This is an update to the alarm @@ -127,7 +127,7 @@ TEST(ListenerTest, listenerTest) listener.addRaiseCb(raise_cb); // Use this overload for any severity raise listener.addClearCb(clear_cb); // Called for any clear of the alarm - ros::Duration latency(0.01); // Approximate upper bound on publisher latency + ros::Duration latency(0.01); // Approximate upper bound on publisher latency auto update_start = update_count; auto lo_start = lo_priority_raise_count; auto hi_start = hi_priority_raise_count; diff --git a/mil_common/ros_alarms/test/rospy/python_tests.py b/mil_common/ros_alarms/test/rospy/python_tests.py index 9c29a7305..c11b0f061 100755 --- a/mil_common/ros_alarms/test/rospy/python_tests.py +++ b/mil_common/ros_alarms/test/rospy/python_tests.py @@ -17,7 +17,7 @@ def __init__(self, *args): super().__init__(*args) """ Tests alarm client operations - Creates some listeners and some broadcasters and tests various raising and clearing conditoins + Creates some listeners and some broadcasters and tests various raising and clearing conditions Also tests various combination of parameters """ @@ -142,7 +142,7 @@ def test_callbacks(self): self.cleared = False self.both = False - # Make sure callbacks run when they're suppsed to + # Make sure callbacks run when they're supposed to ab_a.raise_alarm() rospy.sleep(0.5) diff --git a/mil_common/utils/mil_tools/mil_misc_tools/__init__.py b/mil_common/utils/mil_tools/mil_misc_tools/__init__.py index f2c5ac23f..6af3874c4 100644 --- a/mil_common/utils/mil_tools/mil_misc_tools/__init__.py +++ b/mil_common/utils/mil_tools/mil_misc_tools/__init__.py @@ -4,3 +4,4 @@ from .system_tools import * from .terminal_input import * from .text_effects import * +from .times import datetime_to_rospy, rospy_to_datetime diff --git a/mil_common/utils/mil_tools/mil_misc_tools/download.py b/mil_common/utils/mil_tools/mil_misc_tools/download.py index 85001f69a..fbf3fe61d 100644 --- a/mil_common/utils/mil_tools/mil_misc_tools/download.py +++ b/mil_common/utils/mil_tools/mil_misc_tools/download.py @@ -11,6 +11,7 @@ [3] Download a file via http http://stackoverflow.com/questions/22676 """ + import io as StringIO import os import urllib.request diff --git a/mil_common/utils/mil_tools/mil_misc_tools/times.py b/mil_common/utils/mil_tools/mil_misc_tools/times.py new file mode 100644 index 000000000..554e8fbd8 --- /dev/null +++ b/mil_common/utils/mil_tools/mil_misc_tools/times.py @@ -0,0 +1,11 @@ +import datetime + +import rospy + + +def rospy_to_datetime(time: rospy.Time) -> datetime.datetime: + return datetime.datetime.utcfromtimestamp(time.to_sec()) + + +def datetime_to_rospy(dt: datetime.datetime) -> rospy.Time: + return rospy.Time.from_sec(dt.replace(tzinfo=datetime.timezone.utc).timestamp()) diff --git a/mil_common/utils/mil_tools/mil_ros_tools/cv_debug.py b/mil_common/utils/mil_tools/mil_ros_tools/cv_debug.py index 275abf969..b5695ded9 100644 --- a/mil_common/utils/mil_tools/mil_ros_tools/cv_debug.py +++ b/mil_common/utils/mil_tools/mil_ros_tools/cv_debug.py @@ -1,6 +1,7 @@ """ Shows images for debugging purposes. """ + import sys from typing import Optional diff --git a/mil_common/utils/mil_tools/mil_ros_tools/extract_bag_images.py b/mil_common/utils/mil_tools/mil_ros_tools/extract_bag_images.py index c243ae950..22e52987c 100755 --- a/mil_common/utils/mil_tools/mil_ros_tools/extract_bag_images.py +++ b/mil_common/utils/mil_tools/mil_ros_tools/extract_bag_images.py @@ -150,17 +150,11 @@ def get_camera_model(bag: str, topic: str): _, msg, _ = next(bag.read_messages(topics=camera_info_topic)) except StopIteration: raise Exception( - "no camera info messages found on topic {} in {}".format( - camera_info_topic, - bag, - ), + f"no camera info messages found on topic {camera_info_topic} in {bag}", ) if msg._type != "sensor_msgs/CameraInfo": raise Exception( - "msg on topic {} are not camera info in bag {}".format( - camera_info_topic, - bag, - ), + f"msg on topic {camera_info_topic} are not camera info in bag {bag}", ) model = PinholeCameraModel() model.fromCameraInfo(msg) @@ -218,10 +212,7 @@ def extract_images(self, source_dir=".", image_dir=".", verbose=False): """ if verbose: print( - "\tExtracting images from topic {} in {}".format( - self.topic, - self.filename, - ), + f"\tExtracting images from topic {self.topic} in {self.filename}", ) filename = os.path.join(source_dir, self.filename) b = rosbag.Bag(filename) diff --git a/mil_common/utils/mil_tools/mil_ros_tools/image_helpers.py b/mil_common/utils/mil_tools/mil_ros_tools/image_helpers.py index 22f8eac56..45e12b931 100644 --- a/mil_common/utils/mil_tools/mil_ros_tools/image_helpers.py +++ b/mil_common/utils/mil_tools/mil_ros_tools/image_helpers.py @@ -98,7 +98,7 @@ class Image_Subscriber: last_image_time (genpy.Time): The time of the last image received. im_sub (rospy.Subscriber): The subscriber to the image topic. The topic name and queue size are received through the constructor. - info_sub (rospy.Susbcriber): The subscriber to the camera info topic. + info_sub (rospy.Subscriber): The subscriber to the camera info topic. The topic name is derived from the root of the supplied topic and the queue size is derived from the constructor. bridge (CvBridge): The bridge between OpenCV and ROS. diff --git a/mil_common/utils/mil_tools/mil_ros_tools/init_helpers.py b/mil_common/utils/mil_tools/mil_ros_tools/init_helpers.py index 0a8541fd6..789b36153 100644 --- a/mil_common/utils/mil_tools/mil_ros_tools/init_helpers.py +++ b/mil_common/utils/mil_tools/mil_ros_tools/init_helpers.py @@ -2,6 +2,7 @@ This module provides functions which help to ensure that resources are available when needed. """ + import time from typing import Any, Optional diff --git a/mil_common/utils/mil_tools/mil_ros_tools/label_me_bag.py b/mil_common/utils/mil_tools/mil_ros_tools/label_me_bag.py index d3a98fa67..aa7f91142 100755 --- a/mil_common/utils/mil_tools/mil_ros_tools/label_me_bag.py +++ b/mil_common/utils/mil_tools/mil_ros_tools/label_me_bag.py @@ -53,13 +53,13 @@ def __init__(self, config): self.topics = config["topics"] if not isinstance(self.topics, list): self.topics = [self.topics] - self.start = config["start"] if "start" in config else None - self.stop = config["stop"] if "stop" in config else None - self.freq = config["freq"] if "freq" in config else None + self.start = config.get("start") + self.stop = config.get("stop") + self.freq = config.get("freq") self.name = ( config["name"] if "name" in config else self.default_name(self.filename) ) - self.outfile = config["outfile"] if "outfile" in config else self.filename + self.outfile = config.get("outfile", self.filename) class BagToLabelMe: @@ -263,10 +263,7 @@ def completion_report(self): total_img_count += i if total_img_count == 0: print( - "{}/{} TOTAL images labeled (0%)".format( - total_xml_count, - total_img_count, - ), + f"{total_xml_count}/{total_img_count} TOTAL images labeled (0%)", ) else: print( diff --git a/mil_common/utils/mil_tools/mil_ros_tools/msg_helpers.py b/mil_common/utils/mil_tools/mil_ros_tools/msg_helpers.py index 9ffa9b2d9..8ab315818 100644 --- a/mil_common/utils/mil_tools/mil_ros_tools/msg_helpers.py +++ b/mil_common/utils/mil_tools/mil_ros_tools/msg_helpers.py @@ -58,10 +58,7 @@ def rosmsg_to_numpy(rosmsg, keys=None): break assert ( len(output_array) != 0 - ), "Input type {} has none of these attributes {}.".format( - type(rosmsg).__name__, - keys, - ) + ), f"Input type {type(rosmsg).__name__} has none of these attributes {keys}." return np.array(output_array).astype(np.float32) else: diff --git a/mil_common/utils/mil_tools/nodes/online_bagger.py b/mil_common/utils/mil_tools/nodes/online_bagger.py index 2580f5cf2..5055d3a5f 100755 --- a/mil_common/utils/mil_tools/nodes/online_bagger.py +++ b/mil_common/utils/mil_tools/nodes/online_bagger.py @@ -1,11 +1,13 @@ #!/usr/bin/env python3 +from __future__ import annotations import argparse import datetime import itertools import os from collections import deque +from typing import TYPE_CHECKING import rosbag import rospy @@ -19,20 +21,30 @@ ) from tqdm import tqdm -""" -Online Bagger is a node which subscribes to a list of ros topics, -maintaining a buffer of the most recent n seconds. Parts or all of -these buffered topics can be written to a bag file by -sending a new goal to the /online_bagger/bag action server. - -When run with the -c flag, instead runs an action client which connects -to online bagger, triggering a bag write and displaying a progress bar -as it writes. - -""" +if TYPE_CHECKING: + import genpy class OnlineBagger: + """ + Node that maintains a list of bagged information relating to the specified + topics. + + Subscribes to a list of ROS topics, and maintains a buffer of the most recent + n seconds. Parts or all of these buffered topics can be written to a bag + file by sending a new goal to the /online_bagger/bag action server. When + run with the -c flag, instead runs an action client which connects to online + bagger, triggering a bag write and displaying a progress bar as it writes. + + Attributes: + successful_subscription_count (int): The number of successful subscriptions + to topics. + iteration_count (int): The number of iterations. + streaming (bool): Indicates whether the bagger is streaming. + subscriber_list (list[str]): The list of topics subscribed to the + OnlineBagger. + """ + BAG_TOPIC = "/online_bagger/bag" def __init__(self): @@ -67,14 +79,20 @@ def get_subscriber_list(self, status): """ Get string of all topics, if their subscribe status matches the input (True / False) Outputs each topics: time_buffer(float in seconds), subscribe_statue(bool), topic(string) + + Args: + status (bool): The subscription status used to search for topics with a matching + subscription status. + + Returns: + string: The list of topics that match the desired subscribe status. Each + line in the list contains the buffer time (in seconds) of the topic, the subscrition + status of the topic, and the topic name. """ sub_list = "" for topic in self.subscriber_list: if self.subscriber_list[topic][1] == status: - sub_list = sub_list + "\n{:13}, {}".format( - self.subscriber_list[topic], - topic, - ) + sub_list = sub_list + f"\n{self.subscriber_list[topic]:13}, {topic}" return sub_list def get_params(self): @@ -102,10 +120,23 @@ def get_params(self): self.subscriber_list[topic[0]] = (time, False) def add_unique_topic(topic): + """ + Adds a topic to the subscriber list if the topic is not already in the + list. + + Args: + topic (str): The name of the topic to add to the subscriber list. + """ if topic not in self.subscriber_list: self.subscriber_list[topic] = (self.stream_time, False) def add_env_var(var): + """ + Adds topic(s) to the subscriber list. + + Args: + var (str): The topic(s) to add to the subscriber list. + """ for topic in var.split(): add_unique_topic(topic) @@ -192,7 +223,7 @@ def subscribe_loop(self): self.subscribe, ) - def subscribe(self, time_info=None): + def subscribe(self, _: rospy.timer.TimerEvent | None = None): """ Subscribe to the topics defined in the yaml configuration file @@ -203,13 +234,12 @@ def subscribe(self, time_info=None): Each element in self.subscriber list is a list [topic, Bool] where the Bool tells the current status of the subscriber (success/failure). - - Return number of topics that failed subscription """ - if self.successful_subscription_count == len(self.subscriber_list): - if self.resubscriber is not None: - self.resubscriber.shutdown() - rospy.loginfo("All topics subscribed too! Shutting down resubscriber") + if (self.successful_subscription_count == len(self.subscriber_list)) and ( + self.resubscriber is not None + ): + self.resubscriber.shutdown() + rospy.loginfo("All topics subscribed too! Shutting down resubscriber") for topic, (time, subscribed) in self.subscriber_list.items(): if not subscribed: @@ -224,18 +254,29 @@ def subscribe(self, time_info=None): self.subscriber_list[topic] = (time, True) - def get_topic_duration(self, topic): - """ - Return current time duration of topic + def get_topic_duration(self, topic: str): """ + Returns the current time duration of topic + Args: + topic (str): The topic for which the duration will be calculated. + + Returns: + Duration: The time duration of the topic. + """ return self.topic_messages[topic][-1][0] - self.topic_messages[topic][0][0] - def get_header_time(self, msg): + def get_header_time(self, msg: genpy.Message): """ Retrieve header time if available - """ + Args: + msg (genpy.Message): The ROS message from which to extract the time. + + Returns: + rospy.Time: The timestamp of the topic's header if the topic + has a header. Otherwise, the current time is returned. + """ if hasattr(msg, "header"): return msg.header.stamp else: @@ -243,7 +284,8 @@ def get_header_time(self, msg): def get_time_index(self, topic, requested_seconds): """ - Return the index for the time index for a topic at 'n' seconds from the end of the dequeue + Returns the index for the time index for a topic at 'n' seconds from the end of the dequeue. + For example, to bag the last 10 seconds of data, the index for 10 seconds back from the most recent message can be obtained with this function. The number of requested seconds should be the number of seoncds desired from @@ -251,6 +293,15 @@ def get_time_index(self, topic, requested_seconds): If the desired time length of the bag is greater than the available messages it will output a message and return how ever many seconds of data are available at the moment. Seconds is of a number type (not a rospy.Time type) (ie. int, float) + + Args: + topic (str): The name of the topic for which to get the time index. + requested_seconds (int/float): The number of seconds from the end of the dequeue to search + for the topic. + + Returns: + int: The index for the time index of the topic at requested_seconds seconds from the + end of the dequeue. """ topic_duration = self.get_topic_duration(topic).to_sec() @@ -262,12 +313,17 @@ def get_time_index(self, topic, requested_seconds): index = int(self.get_topic_message_count(topic) * (1 - min(ratio, 1))) return index - def bagger_callback(self, msg, topic): + def bagger_callback(self, msg: genpy.Message, topic: str): """ - Streaming callback function, stops streaming during bagging process - also pops off msgs from dequeue if stream size is greater than specified stream_time + Adds incoming messages to the appropriate topic and removes older messages if necessary. + + Streaming callback function, stops streaming during bagging process and pops off msgs + from dequeue if stream size is greater than specified stream_time. Stream, callback + function does nothing if streaming is not active. - Stream, callback function does nothing if streaming is not active + Args: + msg (genpy.Message): The incoming message. + topic (str): The topic to which the incoming message will be added. """ if not self.streaming: @@ -297,9 +353,16 @@ def bagger_callback(self, msg, topic): self.topic_messages[topic].popleft() time_diff = self.get_topic_duration(topic) - def get_topic_message_count(self, topic): + def get_topic_message_count(self, topic: str): """ Return number of messages available in a topic + + Args: + topic (str): The name of the topic for which to calculate the number + of messages. + + Returns: + int: The number of messages available in the specified topic. """ return len(self.topic_messages[topic]) @@ -307,8 +370,10 @@ def get_topic_message_count(self, topic): def get_total_message_count(self): """ Returns total number of messages across all topics - """ + Returns: + int: The total number of messages available in all topics. + """ total_message_count = 0 for topic in self.topic_messages: total_message_count = total_message_count + self.get_topic_message_count( @@ -318,14 +383,28 @@ def get_total_message_count(self): return total_message_count def _get_default_filename(self): + """ + Uses the current date and time to create a default bag name. + + Returns: + str: The default bag name constructed using format date-time. + """ return ( str(datetime.date.today()) + "-" + str(datetime.datetime.now().time())[0:8] ) - def get_bag_name(self, filename=""): + def get_bag_name(self, filename: str = ""): """ Create ros bag save directory If no bag name is provided, the current date/time is used as default. + + Args: + filename (str): The save directory for the ros bag. The default value + is an empty string, which will result in the default filename being + used. + + Returns: + str: A string representing the path of the ros bag file. """ # If directory param is not set, default to $HOME/bags/ default_dir = self.dir @@ -350,11 +429,19 @@ def get_bag_name(self, filename=""): bag_name = bag_name + ".bag" return os.path.join(bag_dir, bag_name) - def start_bagging(self, req): + def start_bagging(self, req: BagOnlineGoal): """ + Writes collected data to a bag file. + Dump all data in dictionary to bags, temporarily stops streaming during the bagging process, resumes streaming when over. - If bagging is already false because of an active call to this service + If bagging is already false because of an active call to this service. + + Args: + req (BagOnlineGoal): The bagging request information. + + Raises: + IOError: A problem occurs when opening or closing the bag file. """ result = BagOnlineResult() if self.streaming is False: diff --git a/mil_common/utils/mil_tools/nodes/tf_fudger.py b/mil_common/utils/mil_tools/nodes/tf_fudger.py index 58a647757..0ae508c98 100755 --- a/mil_common/utils/mil_tools/nodes/tf_fudger.py +++ b/mil_common/utils/mil_tools/nodes/tf_fudger.py @@ -45,10 +45,7 @@ ang_res = ang_range / ang_max print( - "Distance resolution: {} meters\nAngular resolution: {} radians".format( - x_res, - ang_res, - ), + f"Distance resolution: {x_res} meters\nAngular resolution: {ang_res} radians", ) cv2.createTrackbar("x", "tf", 0, x_max, lambda x: x) diff --git a/mil_common/utils/mil_tools/nodes/tf_to_gazebo.py b/mil_common/utils/mil_tools/nodes/tf_to_gazebo.py index b587dfb88..9363d3c4f 100755 --- a/mil_common/utils/mil_tools/nodes/tf_to_gazebo.py +++ b/mil_common/utils/mil_tools/nodes/tf_to_gazebo.py @@ -25,12 +25,7 @@ def main(): if do_cam_fix: rot = tf.transformations.quaternion_multiply(rot, cam_fix_quat) print( - "(qx={}, qy={} , qz={}, qw={})".format( - rot[0], - rot[1], - rot[2], - rot[3], - ), + f"(qx={rot[0]}, qy={rot[1]} , qz={rot[2]}, qw={rot[3]})", ) print(f"(x={trans[0]}, y={trans[1]}, z={trans[2]})") euler = tf.transformations.euler_from_quaternion(rot) diff --git a/mil_common/utils/mil_tools/test/test_ros_tools.py b/mil_common/utils/mil_tools/test/test_ros_tools.py index e36b967c9..2c0eeeca5 100644 --- a/mil_common/utils/mil_tools/test/test_ros_tools.py +++ b/mil_common/utils/mil_tools/test/test_ros_tools.py @@ -1,8 +1,12 @@ #!/usr/bin/env python3 +import random +import time import unittest import numpy as np +import rospy from geometry_msgs.msg import Pose2D, Quaternion, Vector3 +from mil_misc_tools import datetime_to_rospy, rospy_to_datetime from mil_ros_tools import ( get_image_msg, make_image_msg, @@ -127,10 +131,7 @@ def test_make_rotation(self): [0.0, 0.0, 0.0], np.cross(p_rotated, q), atol=1e-5, - err_msg="The generated rotation matrix did not align the input vectors, {} to {}".format( - p, - q, - ), + err_msg=f"The generated rotation matrix did not align the input vectors, {p} to {q}", ) self.assertGreater( np.dot(p_rotated, q), @@ -156,6 +157,14 @@ def test_normalize_vector(self): err_msg="The normalized vector did not have length 1", ) + def test_datetime_conv(self): + """Test datetime to rospy.Time conversion.""" + for _ in range(10): + unix_stamp = random.randrange(0, int(time.time())) + rp1 = rospy.Time(unix_stamp) + dt1 = rospy_to_datetime(rp1) + self.assertEqual(datetime_to_rospy(dt1), rp1) + if __name__ == "__main__": suite = unittest.TestLoader().loadTestsFromTestCase(TestROSTools) diff --git a/requirements.txt b/requirements.txt index 7a6d801fe..6ff0b0ecc 100644 --- a/requirements.txt +++ b/requirements.txt @@ -11,18 +11,18 @@ pandas==1.4.1 numpy==1.22.3 # Machine Learning -scipy==1.7.3 +scipy==1.10.0 scikit-learn==1.1.1 # Computer Vision -Pillow==9.3.0 +Pillow==10.2.0 # File Handling PyYAML==6.0 urdf-parser-py==0.0.4 # Internet -aiohttp==3.8.1 +aiohttp==3.9.2 aiohttp-xmlrpc==1.5.0 # Documentation @@ -48,4 +48,4 @@ torchvision>=0.13.1 opencv-python==4.6.0.66 seaborn==0.11.2 thop==0.1.1.post2207130030 -requests==2.22.0 +requests==2.31.0 diff --git a/ruff.toml b/ruff.toml index 69b8787be..56794e48d 100644 --- a/ruff.toml +++ b/ruff.toml @@ -28,6 +28,7 @@ extend-exclude = [ target-version = "py38" +[lint] ignore = [ "E501", # Line length "F405", # Unknown import @@ -35,6 +36,7 @@ ignore = [ "D212", # we use second line! "D200", # one line docstrings are not shortened! "SIM115", # too many for now... + "RUF012", # too many for now, adds obvious ClassVar statements ] [per-file-ignores] diff --git a/scripts/install.sh b/scripts/install.sh index d1cabb8b1..f9db76c6b 100755 --- a/scripts/install.sh +++ b/scripts/install.sh @@ -185,6 +185,7 @@ EOF # Documentation dependencies mil_system_install python3-pip python3-setuptools +sudo apt reinstall -y python3-pip # Disable "automatic updates" Ubuntu prompt (thanks to https://askubuntu.com/a/610623!) sudo sed -i 's/Prompt=.*/Prompt=never/' /etc/update-manager/release-upgrades @@ -235,11 +236,16 @@ mil_user_install_dependencies() { git \ tmux \ vim \ + htop \ + tmuxinator \ awscli \ net-tools \ cifs-utils \ nmap \ - tmuxinator + fd-find \ + ripgrep \ + fzf \ + aptitude } # Add line to user's bashrc which source the repo's setup files diff --git a/scripts/setup.bash b/scripts/setup.bash index ebf7e3a3e..f164bb84d 100755 --- a/scripts/setup.bash +++ b/scripts/setup.bash @@ -84,6 +84,7 @@ alias search_root='sudo find / \ -print | grep -i' alias search='find . -print | grep -i' +alias fd="fdfind" # Gazebo aliases alias gazebogui="rosrun gazebo_ros gzclient __name:=gzclient" @@ -98,6 +99,15 @@ startxbox() { roslaunch navigator_launch shore.launch } +# catkin_make for one specific package only +RED='\033[0;31m' +cmonly() { + cd ~/catkin_ws || exit + catkin_make --only-pkg-with-deps $1 + cd - >/dev/null || exit + echo "${RED}!! Warning: Future calls to catkin_make will just build the '$1' package. To revert this, ensure you run 'cm' or 'cd ~/catkin_ws && catkin_make -DCATKIN_WHITELIST_PACKAGES=\"\"' when you want to recompile the entire repository." +} + alias xbox=startxbox # PYTHONPATH modifications