From 12c54561469534d32c72857fee7b50a6b71254e7 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 13 Dec 2024 17:13:48 -0800 Subject: [PATCH] Add pre-commit hooks and BSD 3-Clause license (#51) * Add pre-commit and license * Exclude xacro * Remove duplicate license --- .codespell-ignore | 1 + .github/LICENSE_HEADER.md | 2 + .github/PULL_REQUEST_TEMPLATE.md | 16 +++++ .github/workflows/pre-commit.yaml | 44 ++++++++++++ .pre-commit-config.yaml | 69 +++++++++++++++++++ .pylintrc | 1 - LICENSE => LICENSE.md | 2 +- README.md | 24 ++++--- ada_calibrate_camera/README.md | 13 ++-- .../ada_calibrate_camera/__init__.py | 2 + .../calibrate_camera_node.py | 3 + .../camera_calibration.py | 15 ++-- .../ada_calibrate_camera/charuco_detector.py | 3 + .../ada_calibrate_camera/helpers.py | 15 ++-- .../publish_camera_extrinsics_node.py | 5 +- .../calibrate_camera_start.py | 10 +-- .../config/calibrate_camera.yaml | 3 + ada_calibrate_camera/config/calibs/auto.yaml | 3 + .../config/calibs/manual.yaml | 3 + .../publish_camera_extrinsics_launch.xml | 5 ++ ada_calibrate_camera/setup.py | 3 + ada_description/launch/view_ada.launch.py | 3 + ada_description/meshes/arm.dae | 2 +- ada_description/meshes/base.dae | 2 +- ada_description/meshes/finger_distal.dae | 2 +- ada_description/meshes/finger_proximal.dae | 2 +- ada_description/meshes/forearm.dae | 2 +- ada_description/meshes/hand_3finger.dae | 2 +- ada_description/meshes/ring_big.dae | 2 +- ada_description/meshes/ring_small.dae | 2 +- ada_description/meshes/shoulder.dae | 2 +- ada_description/meshes/wrist.dae | 2 +- ada_description/scripts/camera_calibration.py | 5 +- ada_description/urdf/ada.xacro | 6 +- ada_description/urdf/camera/camera.xacro | 10 +-- .../urdf/camera/camera_standalone.xacro | 4 +- ada_description/urdf/camera/d415urdf.xacro | 12 ++-- ada_description/urdf/forque/forque.xacro | 2 +- .../urdf/forque/forque_standalone.xacro | 6 +- .../urdf/j2n6s200_standalone.xacro | 6 +- .../urdf/j2n6s300_standalone.xacro | 6 +- ada_description/urdf/j2n7s300.xacro | 2 +- .../urdf/j2n7s300_standalone.xacro | 6 +- .../urdf/j2s6s200_integration.xacro | 26 +++---- .../urdf/j2s6s200_standalone.xacro | 6 +- .../urdf/j2s6s300_integration.xacro | 24 +++---- .../urdf/j2s6s300_standalone.xacro | 6 +- .../urdf/j2s7s300_standalone.xacro | 6 +- ada_description/urdf/kinova_inertial.xacro | 2 - ada_hardware/ada_hardware.xml | 5 ++ .../ada_hardware_forward_controllers.yaml | 3 + ada_hardware/launch/ada_hardware.launch.py | 3 + ada_hardware/src/jaco2.cpp | 4 +- .../urdf/ada_hardware.ros2_control.xacro | 2 +- ada_imu/README.md | 20 ++++-- ada_imu/ada_imu/__init__.py | 2 + ada_imu/ada_imu/imu_jointstate_publisher.py | 7 +- ada_imu/config/imu_params.yaml | 8 ++- ada_imu/launch/ada_imu.launch.py | 4 +- ada_imu/scripts/imu_read.py | 4 +- ada_imu/setup.py | 3 + ada_imu/test/test_copyright.py | 3 + ada_imu/test/test_pep257.py | 3 + ada_moveit/.setup_assistant | 2 +- ada_moveit/ada_moveit/__init__.py | 2 + ada_moveit/config/ada.urdf.xacro | 2 +- ada_moveit/config/initial_positions.yaml | 5 +- ada_moveit/config/isaac_controllers.yaml | 5 +- ada_moveit/config/joint_limits.yaml | 5 +- ada_moveit/config/kinematics.yaml | 3 + ada_moveit/config/mock_controllers.yaml | 3 + ada_moveit/config/mock_servo.yaml | 3 + ada_moveit/config/moveit_controllers.yaml | 3 + ada_moveit/config/ompl_planning.yaml | 9 ++- ada_moveit/config/pilz_cartesian_limits.yaml | 3 + ada_moveit/config/real_controllers.yaml | 3 + ada_moveit/config/real_servo.yaml | 3 + ada_moveit/config/sensors_3d.yaml | 4 +- ada_moveit/launch/demo.launch.py | 17 ++++- ada_moveit/launch/demo_feeding.launch.py | 3 + ada_moveit/launch/move_group.launch.py | 3 + ada_moveit/launch/moveit_rviz.launch.py | 3 + ada_moveit/launch/rsp.launch.py | 3 + ada_moveit/launch/setup_assistant.launch.py | 3 + ada_moveit/launch/spawn_controllers.launch.py | 3 + .../launch/static_virtual_joint_tfs.launch.py | 3 + ada_moveit/launch/warehouse_db.launch.py | 3 + ada_moveit/scripts/ada_keyboard_teleop.py | 3 + ada_moveit/scripts/servo_republisher.py | 43 ++++++++---- .../ada_watchdog_listener/__init__.py | 3 + .../ada_watchdog_listener.py | 3 + .../ada_watchdog_listener_node.py | 5 +- ada_watchdog_listener/setup.py | 3 + ada_watchdog_listener/test/test_copyright.py | 3 + ada_watchdog_listener/test/test_flake8.py | 3 + ada_watchdog_listener/test/test_pep257.py | 3 + 96 files changed, 480 insertions(+), 156 deletions(-) create mode 100644 .codespell-ignore create mode 100644 .github/LICENSE_HEADER.md create mode 100644 .github/PULL_REQUEST_TEMPLATE.md create mode 100644 .github/workflows/pre-commit.yaml create mode 100644 .pre-commit-config.yaml rename LICENSE => LICENSE.md (96%) diff --git a/.codespell-ignore b/.codespell-ignore new file mode 100644 index 0000000..11fec86 --- /dev/null +++ b/.codespell-ignore @@ -0,0 +1 @@ +ser diff --git a/.github/LICENSE_HEADER.md b/.github/LICENSE_HEADER.md new file mode 100644 index 0000000..95965d3 --- /dev/null +++ b/.github/LICENSE_HEADER.md @@ -0,0 +1,2 @@ +Copyright (c) 2024, Personal Robotics Laboratory +License: BSD 3-Clause. See LICENSE.md file in root directory. diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md new file mode 100644 index 0000000..9b1ac4d --- /dev/null +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -0,0 +1,16 @@ +# Description + +\[TODO: describe, in-detail, what issue this PR addresses and how it addresses it. Link to relevant Github Issues.\] + +# Testing procedure + +\[TODO: describe, in-detail, how you tested this. The procedure must be detailed enough for the reviewer(s) to recreate it.\] + +# Before opening a pull request + +- \[ \] `pre-commit run --all-files` +- \[ \] Run your code through [pylint](https://pylint.readthedocs.io/en/latest/). `pylint --recursive=y --rcfile=.pylintrc .`. All warnings but `fixme` must be addressed. + +# Before Merging + +- \[ \] `Squash & Merge` diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml new file mode 100644 index 0000000..d71693b --- /dev/null +++ b/.github/workflows/pre-commit.yaml @@ -0,0 +1,44 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +name: pre-commit + +on: + push: + branches: + - main + - master + - ros2-devel + pull_request: + workflow_dispatch: + +concurrency: + group: "${{ github.workflow }} @ ${{ github.event.pull_request.head.label || github.head_ref || github.ref }}" + cancel-in-progress: true + +env: + PYTHON_VERSION: "3.10" + +jobs: + pre-commit: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + with: + python-version: ${{ env.PYTHON_VERSION }} + + ## Run pre-commit and try to apply fixes + - name: Run pre-commit + uses: pre-commit/action@v3.0.1 + - name: Apply fixes from pre-commit + uses: pre-commit-ci/lite-action@v1.0.2 + if: always() + # - name: Install dependencies + # run: | + # python -m pip install --upgrade pip + # pip install --force-reinstall pylint==3.1.0 + # pip install overrides + # - name: Analysing the code with pylint + # run: | + # pylint --recursive=y --rcfile=.pylintrc . --disable fixme --disable import-error diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..6c27063 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,69 @@ +# See https://pre-commit.com for more information +# See https://pre-commit.com/hooks.html for more hooks + +repos: + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.5.0 + hooks: + - id: check-added-large-files + - id: check-case-conflict + - id: check-executables-have-shebangs + - id: check-merge-conflict + - id: check-shebang-scripts-are-executable + - id: check-symlinks + - id: check-xml + - id: check-yaml + - id: debug-statements + - id: destroyed-symlinks + - id: detect-private-key + - id: end-of-file-fixer + - id: mixed-line-ending + - id: requirements-txt-fixer + - id: trailing-whitespace + + - repo: https://github.com/Lucas-C/pre-commit-hooks + rev: v1.5.0 + hooks: + - id: insert-license + args: + - --license-file + - .github/LICENSE_HEADER.md + - --use-current-year + types_or: [python, yaml] + + - repo: https://github.com/Lucas-C/pre-commit-hooks + rev: v1.5.0 + hooks: + - id: insert-license + args: + - --license-file + - .github/LICENSE_HEADER.md + - --use-current-year + - --comment-style + - "" + types_or: [xml] + exclude: ".*package\\.xml$|.*\\.xacro$" + + - repo: https://github.com/psf/black + rev: 23.1.0 + hooks: + - id: black + + - repo: https://github.com/lovesegfault/beautysh + rev: v6.2.1 + hooks: + - id: beautysh + + - repo: https://github.com/executablebooks/mdformat + rev: 0.7.16 + hooks: + - id: mdformat + args: + - --number + + - repo: https://github.com/codespell-project/codespell + rev: v2.2.4 + hooks: + - id: codespell + args: + - --ignore-words=.codespell-ignore diff --git a/.pylintrc b/.pylintrc index bcf6632..bba95be 100644 --- a/.pylintrc +++ b/.pylintrc @@ -643,4 +643,3 @@ init-import=no # List of qualified module names which can have objects that can redefine # builtins. redefining-builtins-modules=six.moves,past.builtins,future.builtins,builtins,io - diff --git a/LICENSE b/LICENSE.md similarity index 96% rename from LICENSE rename to LICENSE.md index f39f2d3..49d47d5 100644 --- a/LICENSE +++ b/LICENSE.md @@ -1,6 +1,6 @@ BSD 3-Clause License -Copyright (c) 2023, Personal Robotics Laboratory +Copyright (c) 2024, Personal Robotics Laboratory Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: diff --git a/README.md b/README.md index 28756a8..d9795ba 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,5 @@ # ada_ros2 + ROS2 Hardware Interface and Description for the ADA Robot ## Setup @@ -6,37 +7,42 @@ ROS2 Hardware Interface and Description for the ADA Robot See the [`ada_feeding` top-level README for setup instructions](https://github.com/personalrobotics/ada_feeding/blob/ros2-devel/README.md). ## Running ADA MoveIt + ### RVIZ + 1. Run `ros2 launch ada_moveit demo.launch.py sim:=mock` command from your ROS2 workspace. 2. See here for a [brief guide to using RVIZ to interact with MoveIt](https://moveit.picknik.ai/humble/doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial.html). ### Real + 1. Run `ros2 launch ada_moveit demo.launch.py` command from your ROS2 workspace. If running for feeding specifically (i.e., where the watchdog dying kills the controllers) run `ros2 launch ada_moveit demo_feeding.launch.py`. Make sure the watchdog is running before you launch this node. ### MoveIt Servo -MoveIt Servo allows [real-time arm servoing](https://moveit.picknik.ai/humble/doc/examples/realtime_servo/realtime_servo_tutorial.html) in cartesian space by sending twist commands to the end effector. +MoveIt Servo allows [real-time arm servoing](https://moveit.picknik.ai/humble/doc/examples/realtime_servo/realtime_servo_tutorial.html) in cartesian space by sending twist commands to the end effector. To use Servo with keyboard teleop: + 1. Launch the force-torque sensor: - 1. Sim: `ros2 run ada_feeding dummy_ft_sensor.py` - 2. Real: `ros2 run forque_sensor_hardware forque_sensor_hardware` + 1. Sim: `ros2 run ada_feeding dummy_ft_sensor.py` + 2. Real: `ros2 run forque_sensor_hardware forque_sensor_hardware` 2. Launch MoveIt: - 1. Sim:`ros2 launch ada_moveit demo.launch.py sim:=mock` - 2. Real: `ros2 launch ada_moveit demo.launch.py` + 1. Sim:`ros2 launch ada_moveit demo.launch.py sim:=mock` + 2. Real: `ros2 launch ada_moveit demo.launch.py` 3. Re-tare the F/T sensor: `ros2 service call /wireless_ft/set_bias std_srvs/srv/SetBool "{data: true}"` 4. Enable MoveIt Servo: - 1. Switch Controllers: `ros2 service call /controller_manager/switch_controller controller_manager_msgs/srv/SwitchController "{activate_controllers: [\"jaco_arm_servo_controller\"], deactivate_controllers: [\"jaco_arm_controller\"], start_controllers: [], stop_controllers: [], strictness: 0, start_asap: false, activate_asap: false, timeout: {sec: 0, nanosec: 0}}"` - 2. Toggle Servo On: `ros2 service call /servo_node/start_servo std_srvs/srv/Trigger "{}"` + 1. Switch Controllers: `ros2 service call /controller_manager/switch_controller controller_manager_msgs/srv/SwitchController "{activate_controllers: [\"jaco_arm_servo_controller\"], deactivate_controllers: [\"jaco_arm_controller\"], start_controllers: [], stop_controllers: [], strictness: 0, start_asap: false, activate_asap: false, timeout: {sec: 0, nanosec: 0}}"` + 2. Toggle Servo On: `ros2 service call /servo_node/start_servo std_srvs/srv/Trigger "{}"` 5. Run the keyboard teleop script: `ros2 run ada_moveit ada_keyboard_teleop.py` 6. Follow the on-screen instructions to teleoperate the robot. Note that although cartesian control avoids obstacles in the planning scene, joint control does not. 7. Toggle Servo Off: `ros2 service call /servo_node/stop_servo std_srvs/srv/Trigger "{}"` To create your own Servo client: + 1. Follow steps 1-4 above. 2. Have your client publish Twist commands to `/servo_node/delta_twist_cmds`. Note the following: - 1. For reliable cartesian control when sending angular velocities on the real robot and `lovelace`, ensure the angular velocity is <= 0.3 rad/s in magnitude. Greater angular velocities might change the end effector's position in addition to its orientation. We believe this is because of latencies with MoveIt Servo getting the robot's joint states via the joint state publisher. - 2. Be sure to send 0-velocity Twist messages at the end to stop the robot. + 1. For reliable cartesian control when sending angular velocities on the real robot and `lovelace`, ensure the angular velocity is \<= 0.3 rad/s in magnitude. Greater angular velocities might change the end effector's position in addition to its orientation. We believe this is because of latencies with MoveIt Servo getting the robot's joint states via the joint state publisher. + 2. Be sure to send 0-velocity Twist messages at the end to stop the robot. ## Camera Calibration diff --git a/ada_calibrate_camera/README.md b/ada_calibrate_camera/README.md index dd567a5..fd8115e 100644 --- a/ada_calibrate_camera/README.md +++ b/ada_calibrate_camera/README.md @@ -1,20 +1,23 @@ # ada_camera_calibration This file contains a nodes to do the following: + 1. Calibrate ADA's eye-in-hand system's extrinsics (run every time the eye-in-hand system changes); 2. Publish the transform between ADA's end-effector and the camera (run every time the robot is used with perception). ## Calibrating the Camera's Extrinsics + 1. Be in the `src` directory of your workspace. 2. `python3 src/ada_ros2/ada_calibrate_camera/calibrate_camera_start.py` 3. `screen -r calibrate` - 1. (See here for relevant [`screen`` commands](https://gist.github.com/jctosta/af918e1618682638aa82)) + 1. (See here for relevant [\`screen\`\` commands](https://gist.github.com/jctosta/af918e1618682638aa82)) 4. Follow the instructions on-screen. (Motions are expected to take ~6 mins and collect up to 30 samples) 5. Once it is done, verify it: - 1. Re-build your workspace. - 2. Run `ros2 launch ada_moveit demo.launch.py sim:=mock` - 3. In RVIZ, add an axis for `camera_color_optical_frame` - 4. Verify it looks like the frame is correct. + 1. Re-build your workspace. + 2. Run `ros2 launch ada_moveit demo.launch.py sim:=mock` + 3. In RVIZ, add an axis for `camera_color_optical_frame` + 4. Verify it looks like the frame is correct. ## Publishing the Transform + 1. `ros2 launch ada_calibrate_camera publish_camera_extrinsics_launch.xml` (this is done by default in the `ada_moveit` launchfile). diff --git a/ada_calibrate_camera/ada_calibrate_camera/__init__.py b/ada_calibrate_camera/ada_calibrate_camera/__init__.py index e69de29..94cfe0b 100644 --- a/ada_calibrate_camera/ada_calibrate_camera/__init__.py +++ b/ada_calibrate_camera/ada_calibrate_camera/__init__.py @@ -0,0 +1,2 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. diff --git a/ada_calibrate_camera/ada_calibrate_camera/calibrate_camera_node.py b/ada_calibrate_camera/ada_calibrate_camera/calibrate_camera_node.py index 5c1addf..fbc8ea7 100755 --- a/ada_calibrate_camera/ada_calibrate_camera/calibrate_camera_node.py +++ b/ada_calibrate_camera/ada_calibrate_camera/calibrate_camera_node.py @@ -1,4 +1,7 @@ #!/usr/bin/env python3 +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + """ This module defines a node that calibrates the camera using a charucoboard. """ diff --git a/ada_calibrate_camera/ada_calibrate_camera/camera_calibration.py b/ada_calibrate_camera/ada_calibrate_camera/camera_calibration.py index 640413b..7870aab 100644 --- a/ada_calibrate_camera/ada_calibrate_camera/camera_calibration.py +++ b/ada_calibrate_camera/ada_calibrate_camera/camera_calibration.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + """ This module defines the CameraCalibration class, which allows users to add samples consisting of an RGB image, a gripper2base transform, and a target2cam transform. @@ -201,7 +204,7 @@ def save_sample( The target2cam translation vector. """ # pylint: disable=too-many-arguments - # Necessay to get all the data for this sample. + # Necessary to get all the data for this sample. cv2.imwrite(os.path.join(data_dir, f"{sample_i}_rgb_img.png"), rgb_img) np.savez_compressed( os.path.join(data_dir, f"{sample_i}_sample.npz"), @@ -351,7 +354,7 @@ def compute_calibration( The calibration method that got the least error. """ # pylint: disable=too-many-locals, too-many-branches, too-many-statements, too-many-arguments - # The comaprison to the reference data requires many arguments. + # The comparison to the reference data requires many arguments. if len(self.Rs_gripper2base) < 3: print("Need at least 3 samples to calibrate the camera.", flush=True) @@ -417,7 +420,7 @@ def compute_calibration( ) continue - # Convert to a homogenous transform + # Convert to a homogeneous transform T_cam2gripper = np.eye(4) T_cam2gripper[:3, :3] = R_cam2gripper T_cam2gripper[:3, 3] = t_cam2gripper.reshape((3,)) @@ -427,7 +430,7 @@ def compute_calibration( ts_target2base = [] # pylint: disable=consider-using-enumerate for i in range(len(self.Rs_target2cam)): - # Get the homogenous transform from the gripper to the base + # Get the homogeneous transform from the gripper to the base T_gripper2base = np.eye(4) if self.Rs_gripper2base[i].shape == (3,): T_gripper2base[:3, :3] = R.from_rotvec( @@ -437,7 +440,7 @@ def compute_calibration( T_gripper2base[:3, :3] = self.Rs_gripper2base[i] T_gripper2base[:3, 3] = self.ts_gripper2base[i] - # Get the homogenous transform from the target to the camera + # Get the homogeneous transform from the target to the camera T_target2cam = np.eye(4) if self.Rs_target2cam[i].shape == (3,): T_target2cam[:3, :3] = R.from_rotvec( @@ -447,7 +450,7 @@ def compute_calibration( T_target2cam[:3, :3] = self.Rs_target2cam[i] T_target2cam[:3, 3] = self.ts_target2cam[i] - # Compute the homogenous transform from the target to the base + # Compute the homogeneous transform from the target to the base T_target2base = T_gripper2base @ T_cam2gripper @ T_target2cam # Extract the rotation and translation diff --git a/ada_calibrate_camera/ada_calibrate_camera/charuco_detector.py b/ada_calibrate_camera/ada_calibrate_camera/charuco_detector.py index 5f05cbe..5a1b3a1 100644 --- a/ada_calibrate_camera/ada_calibrate_camera/charuco_detector.py +++ b/ada_calibrate_camera/ada_calibrate_camera/charuco_detector.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + """ This module defines the CharucoDetector class, which is used to detect a charucoboard in an image. """ diff --git a/ada_calibrate_camera/ada_calibrate_camera/helpers.py b/ada_calibrate_camera/ada_calibrate_camera/helpers.py index 1730854..2e04efd 100644 --- a/ada_calibrate_camera/ada_calibrate_camera/helpers.py +++ b/ada_calibrate_camera/ada_calibrate_camera/helpers.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + """ This module contains helper functions for calibrating the camera. """ @@ -50,7 +53,7 @@ def pose_to_rot_trans( def pose_to_matrix(pose: Pose) -> np.ndarray: """ - Convert a Pose message to a homogenous transformation matrix. + Convert a Pose message to a homogeneous transformation matrix. Parameters ---------- @@ -60,7 +63,7 @@ def pose_to_matrix(pose: Pose) -> np.ndarray: Returns ------- np.ndarray - The homogenous transformation matrix. + The homogeneous transformation matrix. """ M = np.eye(4) M[:3, :3] = R.from_quat(ros2_numpy.numpify(pose.orientation)).as_matrix() @@ -70,7 +73,7 @@ def pose_to_matrix(pose: Pose) -> np.ndarray: def transform_to_matrix(transform: Transform) -> np.ndarray: """ - Convert a Transform message to a homogenous transformation matrix. + Convert a Transform message to a homogeneous transformation matrix. Parameters ---------- @@ -80,7 +83,7 @@ def transform_to_matrix(transform: Transform) -> np.ndarray: Returns ------- np.ndarray - The homogenous transformation matrix. + The homogeneous transformation matrix. """ M = np.eye(4) M[:3, :3] = R.from_quat(ros2_numpy.numpify(transform.rotation)).as_matrix() @@ -90,12 +93,12 @@ def transform_to_matrix(transform: Transform) -> np.ndarray: def matrix_to_pose(M: np.ndarray) -> Pose: """ - Convert a homogenous transformation matrix to a Pose message. + Convert a homogeneous transformation matrix to a Pose message. Parameters ---------- M : np.ndarray - The homogenous transformation matrix. + The homogeneous transformation matrix. Returns ------- diff --git a/ada_calibrate_camera/ada_calibrate_camera/publish_camera_extrinsics_node.py b/ada_calibrate_camera/ada_calibrate_camera/publish_camera_extrinsics_node.py index 7287255..5c0687c 100755 --- a/ada_calibrate_camera/ada_calibrate_camera/publish_camera_extrinsics_node.py +++ b/ada_calibrate_camera/ada_calibrate_camera/publish_camera_extrinsics_node.py @@ -1,4 +1,7 @@ -# !/usr/bin/env python3 +#!/usr/bin/env python3 +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + """ This module contains a rclpy ROS2 node that takes in parameters for the translation and rotation (quaternion) as well as the frame_id's and publishes the requested transform. diff --git a/ada_calibrate_camera/calibrate_camera_start.py b/ada_calibrate_camera/calibrate_camera_start.py index 831dd7f..aa41f1d 100644 --- a/ada_calibrate_camera/calibrate_camera_start.py +++ b/ada_calibrate_camera/calibrate_camera_start.py @@ -1,5 +1,7 @@ -#!/usr/bin/env python3 # -*- coding: utf-8 -*- +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + """ This module starts all the screen sessions to run the ada_feeding demo. """ @@ -46,9 +48,7 @@ parser.add_argument( "--dev", action="store_true", - help=( - "If set, show RVIZ in `moveit`." - ), + help=("If set, show RVIZ in `moveit`."), ) parser.add_argument( "--real_domain_id", @@ -165,7 +165,7 @@ async def main(args: argparse.Namespace, pwd: str) -> None: ( "ros2 run ada_calibrate_camera calibrate_camera --ros-args " f"--params-file {os.path.join(pwd, 'src/ada_ros2/ada_calibrate_camera/config/calibrate_camera.yaml')}" - ) + ), ], } close_commands = {} diff --git a/ada_calibrate_camera/config/calibrate_camera.yaml b/ada_calibrate_camera/config/calibrate_camera.yaml index 1739f31..eebe37a 100644 --- a/ada_calibrate_camera/config/calibrate_camera.yaml +++ b/ada_calibrate_camera/config/calibrate_camera.yaml @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + calibrate_camera: ros__parameters: # Frame names diff --git a/ada_calibrate_camera/config/calibs/auto.yaml b/ada_calibrate_camera/config/calibs/auto.yaml index 23407ac..430ba43 100644 --- a/ada_calibrate_camera/config/calibs/auto.yaml +++ b/ada_calibrate_camera/config/calibs/auto.yaml @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # This file was auto-generated by the ada_calibrate_camera node. # It used the following hand-eye calibration method: 'park'. # The rotation error is 0.016407379903910026 and the translation error is 0.009963572232739004. diff --git a/ada_calibrate_camera/config/calibs/manual.yaml b/ada_calibrate_camera/config/calibs/manual.yaml index 1b4dc97..b4cba18 100644 --- a/ada_calibrate_camera/config/calibs/manual.yaml +++ b/ada_calibrate_camera/config/calibs/manual.yaml @@ -2,6 +2,9 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + publish_camera_extrinsics: ros__parameters: child_frame_id: camera_color_optical_frame diff --git a/ada_calibrate_camera/launch/publish_camera_extrinsics_launch.xml b/ada_calibrate_camera/launch/publish_camera_extrinsics_launch.xml index f9264b3..2806baa 100644 --- a/ada_calibrate_camera/launch/publish_camera_extrinsics_launch.xml +++ b/ada_calibrate_camera/launch/publish_camera_extrinsics_launch.xml @@ -1,3 +1,8 @@ + + diff --git a/ada_calibrate_camera/setup.py b/ada_calibrate_camera/setup.py index f5f8347..1c3638f 100644 --- a/ada_calibrate_camera/setup.py +++ b/ada_calibrate_camera/setup.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + from glob import glob import os from setuptools import find_packages, setup diff --git a/ada_description/launch/view_ada.launch.py b/ada_description/launch/view_ada.launch.py index 269cd44..0d05eec 100644 --- a/ada_description/launch/view_ada.launch.py +++ b/ada_description/launch/view_ada.launch.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # Copyright (c) 2021 PickNik, Inc. # # Redistribution and use in source and binary forms, with or without diff --git a/ada_description/meshes/arm.dae b/ada_description/meshes/arm.dae index c62899d..b23ea31 100644 --- a/ada_description/meshes/arm.dae +++ b/ada_description/meshes/arm.dae @@ -201,4 +201,4 @@ - \ No newline at end of file + diff --git a/ada_description/meshes/base.dae b/ada_description/meshes/base.dae index 5de6e7b..399bb55 100644 --- a/ada_description/meshes/base.dae +++ b/ada_description/meshes/base.dae @@ -201,4 +201,4 @@ - \ No newline at end of file + diff --git a/ada_description/meshes/finger_distal.dae b/ada_description/meshes/finger_distal.dae index 0792282..c243137 100644 --- a/ada_description/meshes/finger_distal.dae +++ b/ada_description/meshes/finger_distal.dae @@ -195,4 +195,4 @@ - \ No newline at end of file + diff --git a/ada_description/meshes/finger_proximal.dae b/ada_description/meshes/finger_proximal.dae index a2b2daf..1b1df5e 100644 --- a/ada_description/meshes/finger_proximal.dae +++ b/ada_description/meshes/finger_proximal.dae @@ -195,4 +195,4 @@ - \ No newline at end of file + diff --git a/ada_description/meshes/forearm.dae b/ada_description/meshes/forearm.dae index 18175e1..6d57e07 100644 --- a/ada_description/meshes/forearm.dae +++ b/ada_description/meshes/forearm.dae @@ -201,4 +201,4 @@ - \ No newline at end of file + diff --git a/ada_description/meshes/hand_3finger.dae b/ada_description/meshes/hand_3finger.dae index 5995dc9..590e708 100644 --- a/ada_description/meshes/hand_3finger.dae +++ b/ada_description/meshes/hand_3finger.dae @@ -201,4 +201,4 @@ - \ No newline at end of file + diff --git a/ada_description/meshes/ring_big.dae b/ada_description/meshes/ring_big.dae index cbe68c8..32cbfd2 100644 --- a/ada_description/meshes/ring_big.dae +++ b/ada_description/meshes/ring_big.dae @@ -195,4 +195,4 @@ - \ No newline at end of file + diff --git a/ada_description/meshes/ring_small.dae b/ada_description/meshes/ring_small.dae index d770d2f..97bff9e 100644 --- a/ada_description/meshes/ring_small.dae +++ b/ada_description/meshes/ring_small.dae @@ -195,4 +195,4 @@ - \ No newline at end of file + diff --git a/ada_description/meshes/shoulder.dae b/ada_description/meshes/shoulder.dae index 20c7f93..cffe726 100644 --- a/ada_description/meshes/shoulder.dae +++ b/ada_description/meshes/shoulder.dae @@ -201,4 +201,4 @@ - \ No newline at end of file + diff --git a/ada_description/meshes/wrist.dae b/ada_description/meshes/wrist.dae index 80d1c3a..80420f1 100644 --- a/ada_description/meshes/wrist.dae +++ b/ada_description/meshes/wrist.dae @@ -201,4 +201,4 @@ - \ No newline at end of file + diff --git a/ada_description/scripts/camera_calibration.py b/ada_description/scripts/camera_calibration.py index 25c0119..df1e160 100755 --- a/ada_description/scripts/camera_calibration.py +++ b/ada_description/scripts/camera_calibration.py @@ -1,4 +1,7 @@ #!/usr/bin/env python3 +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # Adapted from: # https://github.com/crigroup/handeye """ @@ -186,7 +189,7 @@ def del_last_image(self): def add_image(self): """ - Collect the current image trasform + Collect the current image transform """ self.get_logger().info("Saving Calibration Transform") with self.latest_image_lock: diff --git a/ada_description/urdf/ada.xacro b/ada_description/urdf/ada.xacro index 166c371..9eb7a14 100644 --- a/ada_description/urdf/ada.xacro +++ b/ada_description/urdf/ada.xacro @@ -21,10 +21,10 @@ - + - + @@ -32,7 +32,7 @@ lower and upper limits generously encompass the range of the wheelchair's tilt--> - + diff --git a/ada_description/urdf/camera/camera.xacro b/ada_description/urdf/camera/camera.xacro index fd9d381..0f1f2f9 100644 --- a/ada_description/urdf/camera/camera.xacro +++ b/ada_description/urdf/camera/camera.xacro @@ -18,7 +18,7 @@ - + @@ -75,7 +75,7 @@ - + @@ -85,12 +85,12 @@ - + - + @@ -114,4 +114,4 @@ - \ No newline at end of file + diff --git a/ada_description/urdf/camera/camera_standalone.xacro b/ada_description/urdf/camera/camera_standalone.xacro index 3dbc1cc..71665cd 100644 --- a/ada_description/urdf/camera/camera_standalone.xacro +++ b/ada_description/urdf/camera/camera_standalone.xacro @@ -20,12 +20,12 @@ - + - + diff --git a/ada_description/urdf/camera/d415urdf.xacro b/ada_description/urdf/camera/d415urdf.xacro index 9e412b0..4c0cec4 100644 --- a/ada_description/urdf/camera/d415urdf.xacro +++ b/ada_description/urdf/camera/d415urdf.xacro @@ -17,7 +17,7 @@ @@ -39,15 +39,15 @@ aluminum peripherial evaluation case. - + peripheral camera tripod mount --> - @@ -82,7 +82,7 @@ aluminum peripherial evaluation case. - + @@ -166,4 +166,4 @@ aluminum peripherial evaluation case. - \ No newline at end of file + diff --git a/ada_description/urdf/forque/forque.xacro b/ada_description/urdf/forque/forque.xacro index dcf4bfb..bee27dc 100644 --- a/ada_description/urdf/forque/forque.xacro +++ b/ada_description/urdf/forque/forque.xacro @@ -110,4 +110,4 @@ - \ No newline at end of file + diff --git a/ada_description/urdf/forque/forque_standalone.xacro b/ada_description/urdf/forque/forque_standalone.xacro index 278e357..d828dc5 100644 --- a/ada_description/urdf/forque/forque_standalone.xacro +++ b/ada_description/urdf/forque/forque_standalone.xacro @@ -20,16 +20,16 @@ - + - + - \ No newline at end of file + diff --git a/ada_description/urdf/j2n6s200_standalone.xacro b/ada_description/urdf/j2n6s200_standalone.xacro index f91ed80..a6e493d 100644 --- a/ada_description/urdf/j2n6s200_standalone.xacro +++ b/ada_description/urdf/j2n6s200_standalone.xacro @@ -23,12 +23,12 @@ - + - - + + diff --git a/ada_description/urdf/j2n6s300_standalone.xacro b/ada_description/urdf/j2n6s300_standalone.xacro index 984b6b3..15adaf9 100644 --- a/ada_description/urdf/j2n6s300_standalone.xacro +++ b/ada_description/urdf/j2n6s300_standalone.xacro @@ -23,12 +23,12 @@ - + - - + + diff --git a/ada_description/urdf/j2n7s300.xacro b/ada_description/urdf/j2n7s300.xacro index e2105d7..2b9b6fc 100644 --- a/ada_description/urdf/j2n7s300.xacro +++ b/ada_description/urdf/j2n7s300.xacro @@ -91,7 +91,7 @@ - + diff --git a/ada_description/urdf/j2n7s300_standalone.xacro b/ada_description/urdf/j2n7s300_standalone.xacro index 4246f0f..f8fe6fe 100644 --- a/ada_description/urdf/j2n7s300_standalone.xacro +++ b/ada_description/urdf/j2n7s300_standalone.xacro @@ -23,12 +23,12 @@ - + - - + + diff --git a/ada_description/urdf/j2s6s200_integration.xacro b/ada_description/urdf/j2s6s200_integration.xacro index 68bb387..6e7eaca 100644 --- a/ada_description/urdf/j2s6s200_integration.xacro +++ b/ada_description/urdf/j2s6s200_integration.xacro @@ -1,18 +1,18 @@ - @@ -22,4 +22,4 @@ - \ No newline at end of file + diff --git a/ada_description/urdf/j2s6s200_standalone.xacro b/ada_description/urdf/j2s6s200_standalone.xacro index 8a44a03..453541a 100644 --- a/ada_description/urdf/j2s6s200_standalone.xacro +++ b/ada_description/urdf/j2s6s200_standalone.xacro @@ -23,12 +23,12 @@ - + - - + + diff --git a/ada_description/urdf/j2s6s300_integration.xacro b/ada_description/urdf/j2s6s300_integration.xacro index e5ae5f4..ebff262 100644 --- a/ada_description/urdf/j2s6s300_integration.xacro +++ b/ada_description/urdf/j2s6s300_integration.xacro @@ -1,18 +1,18 @@ - diff --git a/ada_description/urdf/j2s6s300_standalone.xacro b/ada_description/urdf/j2s6s300_standalone.xacro index 3dec9ea..05e6c55 100644 --- a/ada_description/urdf/j2s6s300_standalone.xacro +++ b/ada_description/urdf/j2s6s300_standalone.xacro @@ -23,12 +23,12 @@ - + - - + + diff --git a/ada_description/urdf/j2s7s300_standalone.xacro b/ada_description/urdf/j2s7s300_standalone.xacro index c8827cd..4e34906 100644 --- a/ada_description/urdf/j2s7s300_standalone.xacro +++ b/ada_description/urdf/j2s7s300_standalone.xacro @@ -23,12 +23,12 @@ - + - - + + diff --git a/ada_description/urdf/kinova_inertial.xacro b/ada_description/urdf/kinova_inertial.xacro index fc93e0d..80390cb 100644 --- a/ada_description/urdf/kinova_inertial.xacro +++ b/ada_description/urdf/kinova_inertial.xacro @@ -196,5 +196,3 @@ - - diff --git a/ada_hardware/ada_hardware.xml b/ada_hardware/ada_hardware.xml index 2604cb8..9d35dd7 100644 --- a/ada_hardware/ada_hardware.xml +++ b/ada_hardware/ada_hardware.xml @@ -1,3 +1,8 @@ + + - \ No newline at end of file + diff --git a/ada_imu/README.md b/ada_imu/README.md index 968734b..1158427 100644 --- a/ada_imu/README.md +++ b/ada_imu/README.md @@ -1,24 +1,29 @@ # ada_imu + IMU jointstate node for determining and publishing wheelchair tilt ## Dependencies + 1. Install required python packages: `python3 -m pip install pyserial` 2. Remove braille package that conflicts with serial input: `sudo apt remove brltty` ## Running imu_jointstate_publisher + 1. Source your workspace: `source install/setup.bash` 2. Launch the node: `ros2 launch ada_imu ada_imu.launch.py` ## ROS Parameters -* `joint_name`: joint name under which the tilt is published -* `main_calib_vector`: IMU accelerometer reading when robot is level; used to calculate the tilt angle (see Calibration) -* `tilt_calib_vector`: IMU accelerometer reading when robot is tilted; used to establish the plane of rotation to calculate the sign of the tilt angle (see Calibration) -* `serial_port`: serial port IMU outputs to -* `velocity_thresh`: threshold to eliminate velocity noise, if the calculated velocity is less than the threshold, velocity is published as 0 -* `position_smoothing_factor` and `velocity_smoothing_factor`: weights used in exponential smoothing on position and velocity respectively; floating point values between 0 and 1. -* `sim`: If `"real"`, read IMU data from the serial port. Else, assume the IMU angle is always 0. + +- `joint_name`: joint name under which the tilt is published +- `main_calib_vector`: IMU accelerometer reading when robot is level; used to calculate the tilt angle (see Calibration) +- `tilt_calib_vector`: IMU accelerometer reading when robot is tilted; used to establish the plane of rotation to calculate the sign of the tilt angle (see Calibration) +- `serial_port`: serial port IMU outputs to +- `velocity_thresh`: threshold to eliminate velocity noise, if the calculated velocity is less than the threshold, velocity is published as 0 +- `position_smoothing_factor` and `velocity_smoothing_factor`: weights used in exponential smoothing on position and velocity respectively; floating point values between 0 and 1. +- `sim`: If `"real"`, read IMU data from the serial port. Else, assume the IMU angle is always 0. ## Calibration + 1. Level the wheelchair and make sure the robot base is completely perpendicular to the floor 2. cd into `ada_imu/scripts` and run `python3 imu_read.py` 3. Copy the x, y, and z values into the `main_calib_vector` parameter in `config/imu_params.yaml` @@ -27,4 +32,5 @@ IMU jointstate node for determining and publishing wheelchair tilt 6. Copy the x, y, and z values into the `tilt_calib_vector` parameter in `config/imu_params.yaml` ## Hardware + IMU: [OpenLog Artemis](https://learn.sparkfun.com/tutorials/openlog-artemis-hookup-guide/introduction) diff --git a/ada_imu/ada_imu/__init__.py b/ada_imu/ada_imu/__init__.py index e69de29..94cfe0b 100644 --- a/ada_imu/ada_imu/__init__.py +++ b/ada_imu/ada_imu/__init__.py @@ -0,0 +1,2 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. diff --git a/ada_imu/ada_imu/imu_jointstate_publisher.py b/ada_imu/ada_imu/imu_jointstate_publisher.py index 603df9f..056f375 100755 --- a/ada_imu/ada_imu/imu_jointstate_publisher.py +++ b/ada_imu/ada_imu/imu_jointstate_publisher.py @@ -1,9 +1,12 @@ #!/usr/bin/env python3 # -*- coding: utf-8 -*- +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + """ -This module defines a node that continuously reads in accelerometer +This module defines a node that continuously reads in accelerometer data from the IMU to calculate the angle at which the wheelchair is -currently tilted and publishes the angle and velocity to the +currently tilted and publishes the angle and velocity to the /joint_states topic. """ diff --git a/ada_imu/config/imu_params.yaml b/ada_imu/config/imu_params.yaml index 6535479..1b70fb8 100644 --- a/ada_imu/config/imu_params.yaml +++ b/ada_imu/config/imu_params.yaml @@ -1,9 +1,12 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + --- imu_jointstate_publisher: ros__parameters: joint_name: 'robot_tilt' main_calib_vector: # [X, Y, Z] - - -1003.4183333333335 + - -1003.4183333333335 - -24.198333333333334 - 38.275000000000006 @@ -11,11 +14,10 @@ imu_jointstate_publisher: - -978.8416666666667 - -232.50333333333336 - 40.96 - + serial_port: '/dev/ttyUSB0' velocity_thresh: 0.05 # radians per second # weights used in exponential rolling averages, between 0 and 1 position_smoothing_factor: 0.1 velocity_smoothing_factor: 0.1 - diff --git a/ada_imu/launch/ada_imu.launch.py b/ada_imu/launch/ada_imu.launch.py index 1f4f392..c6e9fc5 100644 --- a/ada_imu/launch/ada_imu.launch.py +++ b/ada_imu/launch/ada_imu.launch.py @@ -1,5 +1,7 @@ -#!/usr/bin/env python3 # -*- coding: utf-8 -*- +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + """ Launch file for imu_jointstate_publisher with params from config/imu_params.yaml """ diff --git a/ada_imu/scripts/imu_read.py b/ada_imu/scripts/imu_read.py index 6ab31c0..7ce7f51 100644 --- a/ada_imu/scripts/imu_read.py +++ b/ada_imu/scripts/imu_read.py @@ -1,5 +1,7 @@ -#!/usr/bin/env python3 # -*- coding: utf-8 -*- +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + """ This module provides data for calibrating the IMU jointstate publisher. The main function will read in the IMU's accelerometer data and print diff --git a/ada_imu/setup.py b/ada_imu/setup.py index 672eb95..d68e6b6 100644 --- a/ada_imu/setup.py +++ b/ada_imu/setup.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + from setuptools import setup import os from glob import glob diff --git a/ada_imu/test/test_copyright.py b/ada_imu/test/test_copyright.py index ceffe89..b15eb48 100644 --- a/ada_imu/test/test_copyright.py +++ b/ada_imu/test/test_copyright.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); diff --git a/ada_imu/test/test_pep257.py b/ada_imu/test/test_pep257.py index a2c3deb..ff1f907 100644 --- a/ada_imu/test/test_pep257.py +++ b/ada_imu/test/test_pep257.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); diff --git a/ada_moveit/.setup_assistant b/ada_moveit/.setup_assistant index 7064cde..41a644f 100644 --- a/ada_moveit/.setup_assistant +++ b/ada_moveit/.setup_assistant @@ -28,4 +28,4 @@ moveit_setup_assistant_config: state: - position - velocity - - effort \ No newline at end of file + - effort diff --git a/ada_moveit/ada_moveit/__init__.py b/ada_moveit/ada_moveit/__init__.py index e69de29..94cfe0b 100644 --- a/ada_moveit/ada_moveit/__init__.py +++ b/ada_moveit/ada_moveit/__init__.py @@ -0,0 +1,2 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. diff --git a/ada_moveit/config/ada.urdf.xacro b/ada_moveit/config/ada.urdf.xacro index 69b7e3f..fa76185 100644 --- a/ada_moveit/config/ada.urdf.xacro +++ b/ada_moveit/config/ada.urdf.xacro @@ -10,7 +10,7 @@ - diff --git a/ada_moveit/config/initial_positions.yaml b/ada_moveit/config/initial_positions.yaml index 40ec6e7..fda99f6 100644 --- a/ada_moveit/config/initial_positions.yaml +++ b/ada_moveit/config/initial_positions.yaml @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # Default initial positions for ada's ros2_control fake system initial_positions: @@ -19,4 +22,4 @@ initial_positions: j2n6s200_joint_5: 1.43588 j2n6s200_joint_6: 1.32575 j2n6s200_joint_finger_1: 1.317 - j2n6s200_joint_finger_2: 1.317 \ No newline at end of file + j2n6s200_joint_finger_2: 1.317 diff --git a/ada_moveit/config/isaac_controllers.yaml b/ada_moveit/config/isaac_controllers.yaml index 2379abb..2de069b 100644 --- a/ada_moveit/config/isaac_controllers.yaml +++ b/ada_moveit/config/isaac_controllers.yaml @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # This config file is used by ros2_control controller_manager: ros__parameters: @@ -95,4 +98,4 @@ hand_controller: stopped_velocity_tolerance: 0.01 gains: # Required because we're controlling a velocity interface j2n6s200_joint_finger_1: {p: 1.0, d: 0.0, i: 0.0, i_clamp: 1.0} - j2n6s200_joint_finger_2: {p: 1.0, d: 0.0, i: 0.0, i_clamp: 1.0} \ No newline at end of file + j2n6s200_joint_finger_2: {p: 1.0, d: 0.0, i: 0.0, i_clamp: 1.0} diff --git a/ada_moveit/config/joint_limits.yaml b/ada_moveit/config/joint_limits.yaml index ae7b07b..0e2f1dc 100644 --- a/ada_moveit/config/joint_limits.yaml +++ b/ada_moveit/config/joint_limits.yaml @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed # For beginners, we downscale velocity and acceleration limits. @@ -47,4 +50,4 @@ joint_limits: has_velocity_limits: true max_velocity: 1.0 has_acceleration_limits: false - max_acceleration: 0 \ No newline at end of file + max_acceleration: 0 diff --git a/ada_moveit/config/kinematics.yaml b/ada_moveit/config/kinematics.yaml index f9b2175..a556c84 100644 --- a/ada_moveit/config/kinematics.yaml +++ b/ada_moveit/config/kinematics.yaml @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # Adapted from: https://github.com/PickNikRobotics/pick_ik/blob/main/doc/USAGE.md jaco_arm: kinematics_solver: pick_ik/PickIkPlugin diff --git a/ada_moveit/config/mock_controllers.yaml b/ada_moveit/config/mock_controllers.yaml index 7dc3acc..8963902 100644 --- a/ada_moveit/config/mock_controllers.yaml +++ b/ada_moveit/config/mock_controllers.yaml @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # This config file is used by ros2_control controller_manager: ros__parameters: diff --git a/ada_moveit/config/mock_servo.yaml b/ada_moveit/config/mock_servo.yaml index 07526be..e833f90 100644 --- a/ada_moveit/config/mock_servo.yaml +++ b/ada_moveit/config/mock_servo.yaml @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # Adapted from https://github.com/ros-planning/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config.yaml ############################################### # Modify all parameters related to servoing here diff --git a/ada_moveit/config/moveit_controllers.yaml b/ada_moveit/config/moveit_controllers.yaml index 480d3cc..168592f 100644 --- a/ada_moveit/config/moveit_controllers.yaml +++ b/ada_moveit/config/moveit_controllers.yaml @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # MoveIt uses this configuration for controller management moveit_controller_manager: moveit_ros_control_interface/Ros2ControlManager diff --git a/ada_moveit/config/ompl_planning.yaml b/ada_moveit/config/ompl_planning.yaml index f4cc886..7072746 100644 --- a/ada_moveit/config/ompl_planning.yaml +++ b/ada_moveit/config/ompl_planning.yaml @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # Based on the MoveIt default: https://github.com/ros-planning/moveit2/blob/main/moveit_configs_utils/default_configs/ompl_defaults.yaml planning_plugin: ompl_interface/OMPLPlanner start_state_max_bounds_error: 0.1 @@ -29,7 +32,7 @@ planner_configs: KPIECEkConfigDefault: type: geometric::KPIECE range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 @@ -53,9 +56,9 @@ planner_configs: temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 init_temperature: 10e-6 # initial temperature. default: 10e-6 - frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 - k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() PRMkConfigDefault: type: geometric::PRM max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 diff --git a/ada_moveit/config/pilz_cartesian_limits.yaml b/ada_moveit/config/pilz_cartesian_limits.yaml index b2997ca..670db1e 100644 --- a/ada_moveit/config/pilz_cartesian_limits.yaml +++ b/ada_moveit/config/pilz_cartesian_limits.yaml @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # Limits for the Pilz planner cartesian_limits: max_trans_vel: 1.0 diff --git a/ada_moveit/config/real_controllers.yaml b/ada_moveit/config/real_controllers.yaml index ef29d2e..cdf9edf 100644 --- a/ada_moveit/config/real_controllers.yaml +++ b/ada_moveit/config/real_controllers.yaml @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # This config file is used by ros2_control controller_manager: ros__parameters: diff --git a/ada_moveit/config/real_servo.yaml b/ada_moveit/config/real_servo.yaml index e36551b..04e327e 100644 --- a/ada_moveit/config/real_servo.yaml +++ b/ada_moveit/config/real_servo.yaml @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # Adapted from https://github.com/ros-planning/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config.yaml ############################################### # Modify all parameters related to servoing here diff --git a/ada_moveit/config/sensors_3d.yaml b/ada_moveit/config/sensors_3d.yaml index 1ee0425..6c92739 100644 --- a/ada_moveit/config/sensors_3d.yaml +++ b/ada_moveit/config/sensors_3d.yaml @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + octomap_frame: camera_color_optical_frame octomap_resolution: 0.02 sensors: @@ -13,4 +16,3 @@ default_sensor: padding_offset: 0.03 max_update_rate: 3.0 filtered_cloud_topic: filtered_cloud - diff --git a/ada_moveit/launch/demo.launch.py b/ada_moveit/launch/demo.launch.py index c0793b2..4dbf678 100644 --- a/ada_moveit/launch/demo.launch.py +++ b/ada_moveit/launch/demo.launch.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + import os import yaml from ament_index_python.packages import get_package_share_directory @@ -6,7 +9,10 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource, AnyLaunchDescriptionSource +from launch.launch_description_sources import ( + PythonLaunchDescriptionSource, + AnyLaunchDescriptionSource, +) from launch.substitutions import ( LaunchConfiguration, Command, @@ -93,11 +99,16 @@ def generate_launch_description(): ld.add_action(DeclareBooleanLaunchArg("use_servo", default_value=False)) # Camera Extrinsics Calibration - ada_calibrate_camera_package_path = get_package_share_directory("ada_calibrate_camera") + ada_calibrate_camera_package_path = get_package_share_directory( + "ada_calibrate_camera" + ) ld.add_action( IncludeLaunchDescription( AnyLaunchDescriptionSource( - os.path.join(ada_calibrate_camera_package_path, "launch/publish_camera_extrinsics_launch.xml") + os.path.join( + ada_calibrate_camera_package_path, + "launch/publish_camera_extrinsics_launch.xml", + ) ), launch_arguments={ "log_level": log_level, diff --git a/ada_moveit/launch/demo_feeding.launch.py b/ada_moveit/launch/demo_feeding.launch.py index 5e79e4b..4d7c7da 100644 --- a/ada_moveit/launch/demo_feeding.launch.py +++ b/ada_moveit/launch/demo_feeding.launch.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + from moveit_configs_utils import MoveItConfigsBuilder from launch import LaunchDescription from launch.actions import ( diff --git a/ada_moveit/launch/move_group.launch.py b/ada_moveit/launch/move_group.launch.py index 8fe1795..e687c63 100644 --- a/ada_moveit/launch/move_group.launch.py +++ b/ada_moveit/launch/move_group.launch.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.utilities import normalize_to_list_of_substitutions diff --git a/ada_moveit/launch/moveit_rviz.launch.py b/ada_moveit/launch/moveit_rviz.launch.py index 003213d..a509c56 100644 --- a/ada_moveit/launch/moveit_rviz.launch.py +++ b/ada_moveit/launch/moveit_rviz.launch.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration diff --git a/ada_moveit/launch/rsp.launch.py b/ada_moveit/launch/rsp.launch.py index 38f9495..36f3675 100644 --- a/ada_moveit/launch/rsp.launch.py +++ b/ada_moveit/launch/rsp.launch.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration diff --git a/ada_moveit/launch/setup_assistant.launch.py b/ada_moveit/launch/setup_assistant.launch.py index dd5e28a..82ce714 100644 --- a/ada_moveit/launch/setup_assistant.launch.py +++ b/ada_moveit/launch/setup_assistant.launch.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration diff --git a/ada_moveit/launch/spawn_controllers.launch.py b/ada_moveit/launch/spawn_controllers.launch.py index 8812a82..cae35b7 100644 --- a/ada_moveit/launch/spawn_controllers.launch.py +++ b/ada_moveit/launch/spawn_controllers.launch.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration diff --git a/ada_moveit/launch/static_virtual_joint_tfs.launch.py b/ada_moveit/launch/static_virtual_joint_tfs.launch.py index 16e67bd..ffec852 100644 --- a/ada_moveit/launch/static_virtual_joint_tfs.launch.py +++ b/ada_moveit/launch/static_virtual_joint_tfs.launch.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration diff --git a/ada_moveit/launch/warehouse_db.launch.py b/ada_moveit/launch/warehouse_db.launch.py index 7e9b378..1d3dde3 100644 --- a/ada_moveit/launch/warehouse_db.launch.py +++ b/ada_moveit/launch/warehouse_db.launch.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration diff --git a/ada_moveit/scripts/ada_keyboard_teleop.py b/ada_moveit/scripts/ada_keyboard_teleop.py index 7f5e12f..5d434b7 100755 --- a/ada_moveit/scripts/ada_keyboard_teleop.py +++ b/ada_moveit/scripts/ada_keyboard_teleop.py @@ -1,4 +1,7 @@ #!/usr/bin/env python3 +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # Adapted from: # https://github.com/turtlebot/turtlebot/blob/melodic/turtlebot_teleop/scripts/turtlebot_teleop_key """ diff --git a/ada_moveit/scripts/servo_republisher.py b/ada_moveit/scripts/servo_republisher.py index cf94783..467e3e2 100755 --- a/ada_moveit/scripts/servo_republisher.py +++ b/ada_moveit/scripts/servo_republisher.py @@ -1,4 +1,7 @@ #!/usr/bin/env python3 +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + """ This module defines the ServoRepublisher node, which listens for servo commands (either TwistStamped or JointJog) on input topics, and republishes them on output @@ -230,26 +233,35 @@ def get_joint_controller_interface_name( return # Check if we are already waiting for a response - if (self.joint_controller_get_parameter_future is not None): + if self.joint_controller_get_parameter_future is not None: # If the future is done, wait for the callback to be called if self.joint_controller_get_parameter_future.done(): return # If the future is not done, then wait for it to finish - if (self.get_clock().now() - self.joint_controller_get_parameter_invoke_time) < Duration(seconds=timeout): + if ( + self.get_clock().now() - self.joint_controller_get_parameter_invoke_time + ) < Duration(seconds=timeout): return # If the future is not done and it has been too long, then cancel it - self.joint_controller_get_parameter.remove_pending_request(self.joint_controller_get_parameter_future) - + self.joint_controller_get_parameter.remove_pending_request( + self.joint_controller_get_parameter_future + ) + # Send the request request = GetParameters.Request( names=[parameter_name], ) - self.joint_controller_get_parameter_future = self.joint_controller_get_parameter.call_async(request) + self.joint_controller_get_parameter_future = ( + self.joint_controller_get_parameter.call_async(request) + ) self.joint_controller_get_parameter_invoke_time = self.get_clock().now() - self.joint_controller_get_parameter_future.add_done_callback(self.get_joint_controller_interface_name_callback) - + self.joint_controller_get_parameter_future.add_done_callback( + self.get_joint_controller_interface_name_callback + ) - def get_joint_controller_interface_name_callback(self, future: rclpy.task.Future) -> None: + def get_joint_controller_interface_name_callback( + self, future: rclpy.task.Future + ) -> None: """ Callback for the get_joint_controller_interface_name service call. """ @@ -260,7 +272,7 @@ def get_joint_controller_interface_name_callback(self, future: rclpy.task.Future # If the future succeeded, get the response try: response = future.result() - except Exception as e: # pylint: disable=broad-except + except Exception as e: # pylint: disable=broad-except self.get_logger().error( f"Failed to get the joint controller interface name: {e}" ) @@ -287,7 +299,7 @@ def get_joint_controller_interface_name_callback(self, future: rclpy.task.Future "This node only supports position or velocity joint controllers. " "JointJog messages will be ignored." ) - + # Reset the future self.joint_controller_get_parameter_future = None @@ -418,18 +430,19 @@ def publish_joint_velocity( for i, joint_name in enumerate(JOINT_NAMES): if joint_name in self.latest_joint_states: # Get the joint position - joint_position, joint_state_time = self.latest_joint_states[joint_name] + joint_position, joint_state_time = self.latest_joint_states[ + joint_name + ] if ( self.last_published_joint_positions is not None and self.last_published_joint_positions_time is not None - and joint_state_time < self.last_published_joint_positions_time + and joint_state_time + < self.last_published_joint_positions_time ): # If the last published joint positions are newer than the joint state, use them joint_position = self.last_published_joint_positions[i] # Add the joint position to the list - curr_joint_positions.append( - joint_position - ) + curr_joint_positions.append(joint_position) else: missing_joint_names.append(joint_name) if len(missing_joint_names) > 0: diff --git a/ada_watchdog_listener/ada_watchdog_listener/__init__.py b/ada_watchdog_listener/ada_watchdog_listener/__init__.py index 6f2471a..cf3e199 100644 --- a/ada_watchdog_listener/ada_watchdog_listener/__init__.py +++ b/ada_watchdog_listener/ada_watchdog_listener/__init__.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + """ This package contains the ADAWatchdogListener class and a node that kills itself when the watchdog fails. The latter is useful to shutdown an entire launchfile diff --git a/ada_watchdog_listener/ada_watchdog_listener/ada_watchdog_listener.py b/ada_watchdog_listener/ada_watchdog_listener/ada_watchdog_listener.py index 4872de4..bc32ad3 100644 --- a/ada_watchdog_listener/ada_watchdog_listener/ada_watchdog_listener.py +++ b/ada_watchdog_listener/ada_watchdog_listener/ada_watchdog_listener.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + """ This module defines the ADAWatchdogListener class, which listens to the watchdog topic and can be used in two ways: diff --git a/ada_watchdog_listener/ada_watchdog_listener/ada_watchdog_listener_node.py b/ada_watchdog_listener/ada_watchdog_listener/ada_watchdog_listener_node.py index 1c8bc57..6b55e1d 100755 --- a/ada_watchdog_listener/ada_watchdog_listener/ada_watchdog_listener_node.py +++ b/ada_watchdog_listener/ada_watchdog_listener/ada_watchdog_listener_node.py @@ -1,4 +1,7 @@ #!/usr/bin/env python3 +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + """ This module contains a node, ADAWatchdogListener, which listens to the watchdog topic and kills itself if the watchdog fails. This is useful @@ -66,7 +69,7 @@ def toggle_callback(self, request: SetBool.Request, response: SetBool.Response): response.success = True response.message = f"Successfully set is_on to {request.data}" if request.data: - self.get_logger().info("Succesfully turned the watchdog listener on.") + self.get_logger().info("Successfully turned the watchdog listener on.") else: self.get_logger().warn( "WARNING: You have turned the watchdog listener off. " diff --git a/ada_watchdog_listener/setup.py b/ada_watchdog_listener/setup.py index 4994513..4c9d152 100644 --- a/ada_watchdog_listener/setup.py +++ b/ada_watchdog_listener/setup.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + from setuptools import setup package_name = "ada_watchdog_listener" diff --git a/ada_watchdog_listener/test/test_copyright.py b/ada_watchdog_listener/test/test_copyright.py index ceffe89..b15eb48 100644 --- a/ada_watchdog_listener/test/test_copyright.py +++ b/ada_watchdog_listener/test/test_copyright.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); diff --git a/ada_watchdog_listener/test/test_flake8.py b/ada_watchdog_listener/test/test_flake8.py index ee79f31..2bc2793 100644 --- a/ada_watchdog_listener/test/test_flake8.py +++ b/ada_watchdog_listener/test/test_flake8.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # Copyright 2017 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); diff --git a/ada_watchdog_listener/test/test_pep257.py b/ada_watchdog_listener/test/test_pep257.py index a2c3deb..ff1f907 100644 --- a/ada_watchdog_listener/test/test_pep257.py +++ b/ada_watchdog_listener/test/test_pep257.py @@ -1,3 +1,6 @@ +# Copyright (c) 2024, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License");