diff --git a/.clang-format b/.clang-format index 2593ef54..9f3e95ce 100644 --- a/.clang-format +++ b/.clang-format @@ -1 +1,2 @@ -BasedOnStyle: Google \ No newline at end of file +--- +BasedOnStyle: Google diff --git a/.github/FUNDING.yml b/.github/FUNDING.yml index 011d19c3..9f4855b5 100644 --- a/.github/FUNDING.yml +++ b/.github/FUNDING.yml @@ -1,3 +1,4 @@ +--- github: [senceryazici, ituauvteam] buy_me_a_coffee: ituauvteam custom: ["https://auv.itu.edu.tr/sponsorship.html"] diff --git a/.github/workflows/build_and_push_images.yml b/.github/workflows/build_and_push_images.yml index 06aed17b..7cd996d8 100644 --- a/.github/workflows/build_and_push_images.yml +++ b/.github/workflows/build_and_push_images.yml @@ -1,3 +1,4 @@ +--- name: main docker images on: @@ -7,8 +8,8 @@ on: description: 'Type of image to build [main, base]' required: true type: string - -concurrency: + +concurrency: group: ${{ github.workflow }}-${{ (github.event_name == 'pull_request' && github.event.pull_request.number) || github.ref }} cancel-in-progress: true @@ -21,7 +22,7 @@ env: jobs: build-and-push: runs-on: ubuntu-latest - + strategy: fail-fast: false matrix: @@ -47,7 +48,7 @@ jobs: uses: docker/setup-qemu-action@v3 with: platforms: ${{ matrix.platform }} - + - name: Set up Docker Buildx uses: docker/setup-buildx-action@v3 @@ -98,7 +99,6 @@ jobs: # build-and-push-arm64: # runs-on: self-hosted - # strategy: # matrix: # host: [tegra] @@ -115,7 +115,6 @@ jobs: # uses: docker/setup-qemu-action@v3 # with: # platforms: ${{ matrix.platform }} - # - name: Set up Docker Buildx # uses: docker/setup-buildx-action@v3.3.0 # with: diff --git a/.github/workflows/build_and_push_scheduled.yml b/.github/workflows/build_and_push_scheduled.yml index aaca33ef..f34ae194 100644 --- a/.github/workflows/build_and_push_scheduled.yml +++ b/.github/workflows/build_and_push_scheduled.yml @@ -1,3 +1,4 @@ +--- name: scheduled build and push # scheduled run every day at 00:00 GMT +03:00 @@ -12,7 +13,7 @@ on: schedule: - cron: "0 21 * * *" -concurrency: +concurrency: group: scheduled-${{ github.workflow }}-${{ (github.event_name == 'pull_request' && github.event.pull_request.number) || github.ref }} cancel-in-progress: true diff --git a/.github/workflows/formatting.yml b/.github/workflows/formatting.yml new file mode 100644 index 00000000..a59da7db --- /dev/null +++ b/.github/workflows/formatting.yml @@ -0,0 +1,33 @@ +--- +name: Pre-commit Checks + +on: + workflow_call: + +jobs: + pre-commit: + name: Run Pre-commit Checks + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v3 + + - name: Set up Python + uses: actions/setup-python@v4 + + - name: Install pre-commit + run: | + python -m pip install --upgrade pip + pip install pre-commit + + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y clang-format libxml2-utils + + - name: Run pre-commit + run: pre-commit run --all-files + + - name: Check for modifications + run: git diff --exit-code diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 098c51b8..f9a189f5 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -1,3 +1,4 @@ +--- name: industrial_ci on: @@ -23,7 +24,7 @@ jobs: directory=$(mktemp -d) echo build_directory=$directory >> $GITHUB_ENV - uses: ros-industrial/industrial_ci@master - env: + env: ROS_DISTRO: noetic UPSTREAM_WORKSPACE: third_party.repos ADDITIONAL_DEBS: lld diff --git a/.github/workflows/main_ci.yml b/.github/workflows/main_ci.yml index 95a5cd1b..e4111f2c 100644 --- a/.github/workflows/main_ci.yml +++ b/.github/workflows/main_ci.yml @@ -1,3 +1,4 @@ +--- name: CI on: @@ -6,11 +7,14 @@ on: - main pull_request: -concurrency: +concurrency: group: main-${{ github.workflow }}-${{ (github.event_name == 'pull_request' && github.event.pull_request.number) || github.ref }} cancel-in-progress: true jobs: + formatting: + uses: ./.github/workflows/formatting.yml + build-and-push-base-docker: uses: ./.github/workflows/build_and_push_images.yml with: @@ -26,10 +30,9 @@ jobs: with: runner: ${{ matrix.host }} docker_image_tag: ghcr.io/itu-auv/auv-software-base:latest - + build-and-push-main-docker: needs: [native-ci, build-and-push-base-docker] uses: ./.github/workflows/build_and_push_images.yml with: image_type: main - diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 00000000..34448e52 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,39 @@ +--- +repos: + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v5.0.0 + hooks: + - id: check-yaml + files: \.(yaml|yml)$ + - id: check-xml + files: \.(xml|launch|launch\.xml|sdf|urdf\.xacro|xacro|urdf)$|model\.config$ + - id: end-of-file-fixer + - id: trailing-whitespace + - id: destroyed-symlinks + - id: check-executables-have-shebangs + exclude: \.cpp|\.cxx|\.hpp|\.h$ + - id: forbid-submodules + - id: check-added-large-files + - id: check-case-conflict + - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt + rev: 0.2.3 + hooks: + - id: yamlfmt + args: [--mapping, "2", --sequence, "4", --offset, "2", --width, "150", --preserve-quotes] + exclude: ^auv_bringup/config/ekf.yaml$ + - repo: https://github.com/lsst-ts/pre-commit-xmllint + rev: v1.0.0 + hooks: + - id: format-xmllint + files: \.(xml|launch|launch\.xml|sdf|urdf\.xacro|xacro|urdf)$|model\.config$ + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v19.1.5 + hooks: + - id: clang-format + files: \.(cpp|h|hpp)$ + - repo: https://github.com/psf/black + rev: 24.8.0 + hooks: + - id: black + language_version: python3 + files: \.py$ diff --git a/auv_bringup/config/cameras.yaml b/auv_bringup/config/cameras.yaml index 5a178bc1..2783cb85 100644 --- a/auv_bringup/config/cameras.yaml +++ b/auv_bringup/config/cameras.yaml @@ -1,3 +1,4 @@ +--- cam_front: width: 640 height: 480 @@ -50,4 +51,4 @@ cam_bottom: projection_matrix: rows: 3 cols: 4 - data: [750.047689, 0, 313.903300,0, 0, 988.371962, 250.566244,0, 0, 0, 1,0] + data: [750.047689, 0, 313.903300, 0, 0, 988.371962, 250.566244, 0, 0, 0, 1, 0] diff --git a/auv_bringup/config/environment.yaml b/auv_bringup/config/environment.yaml index df26d7ae..6427b7e7 100644 --- a/auv_bringup/config/environment.yaml +++ b/auv_bringup/config/environment.yaml @@ -1 +1,2 @@ +--- /env/sound_speed: 1500 diff --git a/auv_bringup/config/move_base/costmap_common_params.yaml b/auv_bringup/config/move_base/costmap_common_params.yaml index 45fc31fd..c6534d2c 100644 --- a/auv_bringup/config/move_base/costmap_common_params.yaml +++ b/auv_bringup/config/move_base/costmap_common_params.yaml @@ -1,3 +1,4 @@ +--- obstacle_range: 20.0 raytrace_range: 30.0 @@ -20,4 +21,3 @@ map_type: costmap observation_sources: pointcloud pointcloud: {sensor_frame: taluy/base_link, data_type: PointCloud, topic: pointcloud, marking: true, clearing: true} - diff --git a/auv_bringup/config/move_base/dwa_local_planner_params.yaml b/auv_bringup/config/move_base/dwa_local_planner_params.yaml index 0b8003da..b64d3fe8 100644 --- a/auv_bringup/config/move_base/dwa_local_planner_params.yaml +++ b/auv_bringup/config/move_base/dwa_local_planner_params.yaml @@ -1,3 +1,4 @@ +--- DWAPlannerROS: # Robot Configuration Parameters @@ -16,7 +17,7 @@ DWAPlannerROS: acc_lim_x: 2.0 acc_lim_y: 0.0 - acc_lim_theta: 3.0 + acc_lim_theta: 3.0 acc_lim_trans: 5.0 # Goal Tolerance Parametes @@ -44,5 +45,5 @@ DWAPlannerROS: oscillation_reset_dist: 0.05 # Debugging - publish_traj_pc : true + publish_traj_pc: true publish_cost_grid_pc: true diff --git a/auv_bringup/config/move_base/global_costmap_params.yaml b/auv_bringup/config/move_base/global_costmap_params.yaml index 2a2fb786..25e74c64 100644 --- a/auv_bringup/config/move_base/global_costmap_params.yaml +++ b/auv_bringup/config/move_base/global_costmap_params.yaml @@ -1,3 +1,4 @@ +--- global_costmap: global_frame: odom robot_base_frame: taluy/base_link @@ -11,5 +12,5 @@ global_costmap: resolution: 0.1 static_map: false - rolling_window: true + rolling_window: true track_unknown_space: false diff --git a/auv_bringup/config/move_base/local_costmap_params.yaml b/auv_bringup/config/move_base/local_costmap_params.yaml index 46a48dc4..408732ec 100644 --- a/auv_bringup/config/move_base/local_costmap_params.yaml +++ b/auv_bringup/config/move_base/local_costmap_params.yaml @@ -1,3 +1,4 @@ +--- local_costmap: global_frame: odom robot_base_frame: taluy/base_link @@ -11,4 +12,4 @@ local_costmap: width: 10 height: 10 resolution: 0.01 - track_unknown_space: false + track_unknown_space: false diff --git a/auv_bringup/config/move_base/move_base_params.yaml b/auv_bringup/config/move_base/move_base_params.yaml index 584dbafb..6ac1182b 100644 --- a/auv_bringup/config/move_base/move_base_params.yaml +++ b/auv_bringup/config/move_base/move_base_params.yaml @@ -1,3 +1,4 @@ +--- shutdown_costmaps: false controller_frequency: 10.0 planner_patience: 5.0 diff --git a/auv_bringup/launch/cameras.launch b/auv_bringup/launch/cameras.launch index ec430b28..95489584 100644 --- a/auv_bringup/launch/cameras.launch +++ b/auv_bringup/launch/cameras.launch @@ -11,4 +11,4 @@ - \ No newline at end of file + diff --git a/auv_bringup/launch/inc/boards/start_expansion_board_bridge.launch.xml b/auv_bringup/launch/inc/boards/start_expansion_board_bridge.launch.xml index b7ac4c4a..26ff8a3d 100644 --- a/auv_bringup/launch/inc/boards/start_expansion_board_bridge.launch.xml +++ b/auv_bringup/launch/inc/boards/start_expansion_board_bridge.launch.xml @@ -1,30 +1,24 @@ - - - - - - + + + + + - - + - - - - + + + + - - - - - + + + + - \ No newline at end of file + diff --git a/auv_bringup/launch/inc/imu/start_xsens_driver.launch.xml b/auv_bringup/launch/inc/imu/start_xsens_driver.launch.xml index 0a30847c..5d8e8d05 100644 --- a/auv_bringup/launch/inc/imu/start_xsens_driver.launch.xml +++ b/auv_bringup/launch/inc/imu/start_xsens_driver.launch.xml @@ -1,63 +1,53 @@ - - - - - + + + + - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + - - + + - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + - - - - - - - - + + + + + + + - \ No newline at end of file + diff --git a/auv_bringup/launch/inc/localization/start_robot_localization.launch.xml b/auv_bringup/launch/inc/localization/start_robot_localization.launch.xml index eb8f30bd..e097e3a3 100644 --- a/auv_bringup/launch/inc/localization/start_robot_localization.launch.xml +++ b/auv_bringup/launch/inc/localization/start_robot_localization.launch.xml @@ -1,15 +1,14 @@ + - - - + + - - - - - - + + + + + + - \ No newline at end of file + diff --git a/auv_bringup/launch/inc/localization/start_state_publisher.launch.xml b/auv_bringup/launch/inc/localization/start_state_publisher.launch.xml index 800dfe09..2b8dfa84 100644 --- a/auv_bringup/launch/inc/localization/start_state_publisher.launch.xml +++ b/auv_bringup/launch/inc/localization/start_state_publisher.launch.xml @@ -1,25 +1,18 @@ + - - - - - - + + + + - + - - - + + - - - + + - - + diff --git a/auv_bringup/launch/inc/logging.launch.xml b/auv_bringup/launch/inc/logging.launch.xml index dcb785fb..dcea6a81 100644 --- a/auv_bringup/launch/inc/logging.launch.xml +++ b/auv_bringup/launch/inc/logging.launch.xml @@ -1,15 +1,13 @@ - - - - + + + + - - - + + - + diff --git a/auv_bringup/launch/inc/sensors/start_cameras.launch.xml b/auv_bringup/launch/inc/sensors/start_cameras.launch.xml index 702fb7e4..c15c2dfa 100644 --- a/auv_bringup/launch/inc/sensors/start_cameras.launch.xml +++ b/auv_bringup/launch/inc/sensors/start_cameras.launch.xml @@ -1,40 +1,35 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + - - \ No newline at end of file + --> + diff --git a/auv_bringup/launch/inc/sensors/start_ping360_driver.launch.xml b/auv_bringup/launch/inc/sensors/start_ping360_driver.launch.xml index 0daed288..aa06d60f 100644 --- a/auv_bringup/launch/inc/sensors/start_ping360_driver.launch.xml +++ b/auv_bringup/launch/inc/sensors/start_ping360_driver.launch.xml @@ -1,37 +1,33 @@ - - - - - + + + + - - + - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + - \ No newline at end of file + diff --git a/auv_bringup/launch/inc/sensors/start_wayfinder_dvl_driver.launch.xml b/auv_bringup/launch/inc/sensors/start_wayfinder_dvl_driver.launch.xml index 09a24e61..34e89bd2 100644 --- a/auv_bringup/launch/inc/sensors/start_wayfinder_dvl_driver.launch.xml +++ b/auv_bringup/launch/inc/sensors/start_wayfinder_dvl_driver.launch.xml @@ -1,29 +1,24 @@ - - - - + + + - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + - \ No newline at end of file + diff --git a/auv_bringup/launch/inc/sensors/start_xsens_driver.launch.xml b/auv_bringup/launch/inc/sensors/start_xsens_driver.launch.xml index 0a30847c..5d8e8d05 100644 --- a/auv_bringup/launch/inc/sensors/start_xsens_driver.launch.xml +++ b/auv_bringup/launch/inc/sensors/start_xsens_driver.launch.xml @@ -1,63 +1,53 @@ - - - - - + + + + - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + - - + + - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + - - - - - - - - + + + + + + + - \ No newline at end of file + diff --git a/auv_bringup/launch/robot_localization.launch b/auv_bringup/launch/robot_localization.launch index 08ed3369..e0ccd1e2 100644 --- a/auv_bringup/launch/robot_localization.launch +++ b/auv_bringup/launch/robot_localization.launch @@ -7,4 +7,4 @@ - \ No newline at end of file + diff --git a/auv_bringup/launch/start.launch b/auv_bringup/launch/start.launch index 0811e318..5d31f0f7 100644 --- a/auv_bringup/launch/start.launch +++ b/auv_bringup/launch/start.launch @@ -4,6 +4,7 @@ + @@ -60,7 +61,7 @@ - + diff --git a/auv_bringup/package.xml b/auv_bringup/package.xml index d5724d87..589fd65d 100644 --- a/auv_bringup/package.xml +++ b/auv_bringup/package.xml @@ -3,18 +3,13 @@ auv_bringup 0.1.0 The auv_bringup package - Sencer Yazici Sencer Yazici - BSD-3-Clause - - catkin xsens_mti_driver ping360_sonar robot_localization - diff --git a/auv_cam/CMakeLists.txt b/auv_cam/CMakeLists.txt index 3a450219..b71d104e 100644 --- a/auv_cam/CMakeLists.txt +++ b/auv_cam/CMakeLists.txt @@ -65,11 +65,11 @@ install(TARGETS auv_gst_cam_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(DIRECTORY - launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install(DIRECTORY + launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) \ No newline at end of file +) diff --git a/auv_cam/include/auv_cam/auv_camera_ros.h b/auv_cam/include/auv_cam/auv_camera_ros.h index ba38e102..d053465d 100644 --- a/auv_cam/include/auv_cam/auv_camera_ros.h +++ b/auv_cam/include/auv_cam/auv_camera_ros.h @@ -33,9 +33,9 @@ #include #include +#include #include #include -#include #include "opencv2/opencv.hpp" #include "ros/ros.h" diff --git a/auv_cam/launch/inc/image_proc.launch.xml b/auv_cam/launch/inc/image_proc.launch.xml index 83c6d95e..d65354a3 100644 --- a/auv_cam/launch/inc/image_proc.launch.xml +++ b/auv_cam/launch/inc/image_proc.launch.xml @@ -1,43 +1,27 @@ + - - - - - - + + + + + - - - - - - - - + + + + + - + - - + - - - - + + + - \ No newline at end of file + diff --git a/auv_cam/launch/start_gst_camera.launch b/auv_cam/launch/start_gst_camera.launch index 62ba9da5..ab520f2c 100644 --- a/auv_cam/launch/start_gst_camera.launch +++ b/auv_cam/launch/start_gst_camera.launch @@ -30,4 +30,4 @@ - \ No newline at end of file + diff --git a/auv_cam/package.xml b/auv_cam/package.xml index 7d388a68..305c5276 100644 --- a/auv_cam/package.xml +++ b/auv_cam/package.xml @@ -3,13 +3,9 @@ auv_cam 0.1.0 The auv_cam package - Sencer Yazici - BSD-3-Clause - Sencer Yazici - catkin cv_bridge roscpp @@ -18,10 +14,8 @@ cv_bridge roscpp image_transport - - - \ No newline at end of file + diff --git a/auv_common_lib/include/auv_common_lib/ros/conversions.h b/auv_common_lib/include/auv_common_lib/ros/conversions.h index bea02c8f..ea2f3c5b 100644 --- a/auv_common_lib/include/auv_common_lib/ros/conversions.h +++ b/auv_common_lib/include/auv_common_lib/ros/conversions.h @@ -9,4 +9,4 @@ To convert(const From& from); } // namespace conversions } // namespace common -} // namespace auv \ No newline at end of file +} // namespace auv diff --git a/auv_common_lib/include/auv_common_lib/ros/rosparam.h b/auv_common_lib/include/auv_common_lib/ros/rosparam.h index 8d8d78e1..1e2756dc 100644 --- a/auv_common_lib/include/auv_common_lib/ros/rosparam.h +++ b/auv_common_lib/include/auv_common_lib/ros/rosparam.h @@ -46,4 +46,4 @@ struct parser { } // namespace rosparam } // namespace common -} // namespace auv \ No newline at end of file +} // namespace auv diff --git a/auv_common_lib/include/auv_common_lib/ros/subscriber_with_buffer.h b/auv_common_lib/include/auv_common_lib/ros/subscriber_with_buffer.h index 61ce0508..c5d809ca 100644 --- a/auv_common_lib/include/auv_common_lib/ros/subscriber_with_buffer.h +++ b/auv_common_lib/include/auv_common_lib/ros/subscriber_with_buffer.h @@ -61,4 +61,4 @@ class SubscriberWithBuffer { } // namespace ros } // namespace common -} // namespace auv \ No newline at end of file +} // namespace auv diff --git a/auv_common_lib/include/auv_common_lib/ros/subscriber_with_timeout.h b/auv_common_lib/include/auv_common_lib/ros/subscriber_with_timeout.h index 9b3ae9d9..189dddd6 100644 --- a/auv_common_lib/include/auv_common_lib/ros/subscriber_with_timeout.h +++ b/auv_common_lib/include/auv_common_lib/ros/subscriber_with_timeout.h @@ -89,4 +89,4 @@ class SubscriberWithTimeout : private SubscriberWithBuffer { // } // namespace ros } // namespace common -} // namespace auv \ No newline at end of file +} // namespace auv diff --git a/auv_common_lib/package.xml b/auv_common_lib/package.xml index 3ae4495c..0a391659 100644 --- a/auv_common_lib/package.xml +++ b/auv_common_lib/package.xml @@ -3,16 +3,13 @@ auv_common_lib 0.1.0 The auv_common_lib package - Sencer Yazici Sencer Yazici - BSD-3-Clause - catkin roscpp roscpp roscpp nav_msgs geometry_msgs - \ No newline at end of file + diff --git a/auv_common_lib/python/auv_common_lib/logging/terminal_color_utils.py b/auv_common_lib/python/auv_common_lib/logging/terminal_color_utils.py index 4feacc61..12fefbf2 100644 --- a/auv_common_lib/python/auv_common_lib/logging/terminal_color_utils.py +++ b/auv_common_lib/python/auv_common_lib/logging/terminal_color_utils.py @@ -1,17 +1,17 @@ class TerminalColors: - HEADER = '\033[95m' - OKBLUE = '\033[94m' - OKCYAN = '\033[96m' - OKGREEN = '\033[92m' - OKYELLOW = '\033[93m' - FAIL = '\033[91m' - ENDC = '\033[0m' - BOLD = '\033[1m' - UNDERLINE = '\033[4m' - ORANGE = '\033[38;5;214m' - PASTEL_BLUE = '\033[38;5;110m' - PASTEL_GREEN = '\033[38;5;157m' - PASTEL_PURPLE = '\033[38;5;183m' + HEADER = "\033[95m" + OKBLUE = "\033[94m" + OKCYAN = "\033[96m" + OKGREEN = "\033[92m" + OKYELLOW = "\033[93m" + FAIL = "\033[91m" + ENDC = "\033[0m" + BOLD = "\033[1m" + UNDERLINE = "\033[4m" + ORANGE = "\033[38;5;214m" + PASTEL_BLUE = "\033[38;5;110m" + PASTEL_GREEN = "\033[38;5;157m" + PASTEL_PURPLE = "\033[38;5;183m" @staticmethod def color_text(text, color): @@ -29,6 +29,7 @@ def underline_text(text): def rgb_text(text, r, g, b): return f"\033[38;2;{r};{g};{b}m{text}{TerminalColors.ENDC}" + # Example usage # print(TerminalColors.color_text("This is a pastel blue text.", TerminalColors.PASTEL_BLUE)) # print(TerminalColors.color_text("This is an orange text.", TerminalColors.ORANGE)) diff --git a/auv_common_lib/src/ros/rosparam_matrix.cpp b/auv_common_lib/src/ros/rosparam_matrix.cpp index 64f96424..1e242ae9 100644 --- a/auv_common_lib/src/ros/rosparam_matrix.cpp +++ b/auv_common_lib/src/ros/rosparam_matrix.cpp @@ -124,4 +124,4 @@ Eigen::Matrix detail::parse>( } // namespace rosparam } // namespace common -} // namespace auv \ No newline at end of file +} // namespace auv diff --git a/auv_control/auv_control/CMakeLists.txt b/auv_control/auv_control/CMakeLists.txt index a6a5008c..f78cba73 100644 --- a/auv_control/auv_control/CMakeLists.txt +++ b/auv_control/auv_control/CMakeLists.txt @@ -49,7 +49,7 @@ catkin_install_python(PROGRAMS DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -add_executable(${PROJECT_NAME}_node +add_executable(${PROJECT_NAME}_node src/controller_ros.cpp src/controller_node.cpp ) @@ -65,7 +65,7 @@ install(TARGETS ${PROJECT_NAME}_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -add_executable(thruster_manager_node +add_executable(thruster_manager_node src/thruster_manager_ros.cpp src/thruster_manager_node.cpp ) diff --git a/auv_control/auv_control/config/default.yaml b/auv_control/auv_control/config/default.yaml index c1317a94..51988d0a 100644 --- a/auv_control/auv_control/config/default.yaml +++ b/auv_control/auv_control/config/default.yaml @@ -1,3 +1,4 @@ +--- model: mass_inertia_matrix: - [27.0, 0, 0, 0, 0, 0] @@ -37,4 +38,3 @@ min_thrust: 0.5 # N coeffs_ccw: [12.292935764557399, -3.0745135927968605, -1.0664649834600253, 181.68212348405177, 36.69456713973092, 1151.1307183400536] coeffs_cw: [-7.65890867108129, -2.272619267631361, 1.5070809906898863, 140.79738360070297, -50.325593642656244, 1954.5975713257544] mapping: [4, 0, 2, 6, 7, 3, 5, 1] - diff --git a/auv_control/auv_control/include/auv_control/controller_ros.h b/auv_control/auv_control/include/auv_control/controller_ros.h index fd655e77..e4649ae9 100644 --- a/auv_control/auv_control/include/auv_control/controller_ros.h +++ b/auv_control/auv_control/include/auv_control/controller_ros.h @@ -1,5 +1,8 @@ #pragma once +#include // Include your dynamic reconfigure header +#include + #include #include "auv_common_lib/ros/conversions.h" @@ -7,15 +10,12 @@ #include "auv_common_lib/ros/subscriber_with_timeout.h" #include "auv_controllers/controller_base.h" #include "auv_controllers/multidof_pid_controller.h" -#include "geometry_msgs/Wrench.h" #include "geometry_msgs/AccelWithCovarianceStamped.h" +#include "geometry_msgs/Wrench.h" #include "nav_msgs/Odometry.h" #include "pluginlib/class_loader.h" #include "ros/ros.h" #include "std_msgs/Bool.h" -#include -#include // Include your dynamic reconfigure header - namespace auv { namespace control { @@ -68,16 +68,16 @@ class ControllerROS { f = boost::bind(&ControllerROS::reconfigure_callback, this, _1, _2); dr_srv_.updateConfig(initial_config); // Apply the initial configuration dr_srv_.setCallback(f); - + odometry_sub_ = nh_.subscribe("odometry", 1, &ControllerROS::odometry_callback, this); cmd_vel_sub_ = nh_.subscribe("cmd_vel", 1, &ControllerROS::cmd_vel_callback, this); cmd_pose_sub_ = nh_.subscribe("cmd_pose", 1, &ControllerROS::cmd_pose_callback, this); - accel_sub_ = + accel_sub_ = nh_.subscribe("acceleration", 1, &ControllerROS::accel_callback, this); - + control_enable_sub_.subscribe( "enable", 1, nullptr, []() { ROS_WARN_STREAM("control enable message timeouted"); }, @@ -122,7 +122,6 @@ class ControllerROS { : geometry_msgs::Wrench{}; wrench_pub_.publish(wrench_msg); - } } @@ -154,20 +153,28 @@ class ControllerROS { latest_command_time_ = ros::Time::now(); } - void accel_callback(const geometry_msgs::AccelWithCovarianceStamped::ConstPtr& msg) { + void accel_callback( + const geometry_msgs::AccelWithCovarianceStamped::ConstPtr& msg) { d_state_(6) = msg->accel.accel.linear.x; d_state_(7) = msg->accel.accel.linear.y; d_state_(8) = msg->accel.accel.linear.z; d_state_.tail(3) = Eigen::Vector3d::Zero(); } + void reconfigure_callback(auv_control::ControllerConfig& config, + uint32_t level) { + auto controller = + dynamic_cast(controller_.get()); - void reconfigure_callback(auv_control::ControllerConfig& config, uint32_t level) { - auto controller = dynamic_cast(controller_.get()); - - kp_ << config.kp_0, config.kp_1, config.kp_2, config.kp_3, config.kp_4, config.kp_5, config.kp_6, config.kp_7, config.kp_8, config.kp_9, config.kp_10, config.kp_11; - ki_ << config.ki_0, config.ki_1, config.ki_2, config.ki_3, config.ki_4, config.ki_5, config.ki_6, config.ki_7, config.ki_8, config.ki_9, config.ki_10, config.ki_11; - kd_ << config.kd_0, config.kd_1, config.kd_2, config.kd_3, config.kd_4, config.kd_5, config.kd_6, config.kd_7, config.kd_8, config.kd_9, config.kd_10, config.kd_11; + kp_ << config.kp_0, config.kp_1, config.kp_2, config.kp_3, config.kp_4, + config.kp_5, config.kp_6, config.kp_7, config.kp_8, config.kp_9, + config.kp_10, config.kp_11; + ki_ << config.ki_0, config.ki_1, config.ki_2, config.ki_3, config.ki_4, + config.ki_5, config.ki_6, config.ki_7, config.ki_8, config.ki_9, + config.ki_10, config.ki_11; + kd_ << config.kd_0, config.kd_1, config.kd_2, config.kd_3, config.kd_4, + config.kd_5, config.kd_6, config.kd_7, config.kd_8, config.kd_9, + config.kd_10, config.kd_11; controller->set_kp(kp_); controller->set_ki(ki_); controller->set_kd(kd_); @@ -182,20 +189,47 @@ class ControllerROS { } void set_initial_config(auv_control::ControllerConfig& config) { - config.kp_0 = kp_(0); config.kp_1 = kp_(1); config.kp_2 = kp_(2); config.kp_3 = kp_(3); - config.kp_4 = kp_(4); config.kp_5 = kp_(5); config.kp_6 = kp_(6); config.kp_7 = kp_(7); - config.kp_8 = kp_(8); config.kp_9 = kp_(9); config.kp_10 = kp_(10); config.kp_11 = kp_(11); - - config.ki_0 = ki_(0); config.ki_1 = ki_(1); config.ki_2 = ki_(2); config.ki_3 = ki_(3); - config.ki_4 = ki_(4); config.ki_5 = ki_(5); config.ki_6 = ki_(6); config.ki_7 = ki_(7); - config.ki_8 = ki_(8); config.ki_9 = ki_(9); config.ki_10 = ki_(10); config.ki_11 = ki_(11); - - config.kd_0 = kd_(0); config.kd_1 = kd_(1); config.kd_2 = kd_(2); config.kd_3 = kd_(3); - config.kd_4 = kd_(4); config.kd_5 = kd_(5); config.kd_6 = kd_(6); config.kd_7 = kd_(7); - config.kd_8 = kd_(8); config.kd_9 = kd_(9); config.kd_10 = kd_(10); config.kd_11 = kd_(11); + config.kp_0 = kp_(0); + config.kp_1 = kp_(1); + config.kp_2 = kp_(2); + config.kp_3 = kp_(3); + config.kp_4 = kp_(4); + config.kp_5 = kp_(5); + config.kp_6 = kp_(6); + config.kp_7 = kp_(7); + config.kp_8 = kp_(8); + config.kp_9 = kp_(9); + config.kp_10 = kp_(10); + config.kp_11 = kp_(11); + + config.ki_0 = ki_(0); + config.ki_1 = ki_(1); + config.ki_2 = ki_(2); + config.ki_3 = ki_(3); + config.ki_4 = ki_(4); + config.ki_5 = ki_(5); + config.ki_6 = ki_(6); + config.ki_7 = ki_(7); + config.ki_8 = ki_(8); + config.ki_9 = ki_(9); + config.ki_10 = ki_(10); + config.ki_11 = ki_(11); + + config.kd_0 = kd_(0); + config.kd_1 = kd_(1); + config.kd_2 = kd_(2); + config.kd_3 = kd_(3); + config.kd_4 = kd_(4); + config.kd_5 = kd_(5); + config.kd_6 = kd_(6); + config.kd_7 = kd_(7); + config.kd_8 = kd_(8); + config.kd_9 = kd_(9); + config.kd_10 = kd_(10); + config.kd_11 = kd_(11); } -void save_parameters() { + void save_parameters() { ros::NodeHandle nh_private("~"); nh_private.param("config_file", config_file_, std::string{}); if (config_file_.empty()) { @@ -214,13 +248,14 @@ void save_parameters() { std::string content = buffer.str(); in_file.close(); - auto replace_param = [](std::string& content, const std::string& param, const Eigen::Matrix& values) { + auto replace_param = [](std::string& content, const std::string& param, + const Eigen::Matrix& values) { std::stringstream ss; ss << std::fixed << std::setprecision(1); ss << param << ": [" << values(0); for (int i = 1; i < 12; ++i) ss << ", " << values(i); ss << "]"; - + std::string::size_type start_pos = content.find(param + ": ["); if (start_pos == std::string::npos) { // If parameter not found, add it to the end @@ -237,15 +272,15 @@ void save_parameters() { std::ofstream out_file(config_file_); if (!out_file.is_open()) { - ROS_ERROR_STREAM("Failed to open config file for writing: " << config_file_); + ROS_ERROR_STREAM( + "Failed to open config file for writing: " << config_file_); return; } out_file << content; out_file.close(); ROS_INFO_STREAM("Parameters saved to " << config_file_); -} - + } ros::Rate rate_; ros::NodeHandle nh_; @@ -267,10 +302,12 @@ void save_parameters() { ControllerLoader controller_loader_{"auv_controllers", "auv::control::SixDOFControllerBase"}; - dynamic_reconfigure::Server dr_srv_; // Dynamic reconfigure server - Eigen::Matrix kp_, ki_, kd_; // Parameters to be dynamically reconfigured - std::string config_file_; // Path to the config file + dynamic_reconfigure::Server + dr_srv_; // Dynamic reconfigure server + Eigen::Matrix kp_, ki_, + kd_; // Parameters to be dynamically reconfigured + std::string config_file_; // Path to the config file }; } // namespace control -} // namespace auv \ No newline at end of file +} // namespace auv diff --git a/auv_control/auv_control/include/auv_control/thruster_allocation.h b/auv_control/auv_control/include/auv_control/thruster_allocation.h index e958b319..0bfd8c97 100644 --- a/auv_control/auv_control/include/auv_control/thruster_allocation.h +++ b/auv_control/auv_control/include/auv_control/thruster_allocation.h @@ -148,4 +148,4 @@ class ThrusterAllocator { }; } // namespace control -} // namespace auv \ No newline at end of file +} // namespace auv diff --git a/auv_control/auv_control/launch/start.launch b/auv_control/auv_control/launch/start.launch index e057d65c..1979776c 100644 --- a/auv_control/auv_control/launch/start.launch +++ b/auv_control/auv_control/launch/start.launch @@ -21,7 +21,7 @@ - + @@ -63,4 +63,4 @@ - \ No newline at end of file + diff --git a/auv_control/auv_control/package.xml b/auv_control/auv_control/package.xml index de639142..7c054b67 100644 --- a/auv_control/auv_control/package.xml +++ b/auv_control/auv_control/package.xml @@ -3,11 +3,9 @@ auv_control 0.1.0 The auv_control package - Sencer Yazici Sencer Yazici BSD-3-Clause - catkin roscpp auv_common_lib diff --git a/auv_control/auv_control/scripts/battery_monitor_node.py b/auv_control/auv_control/scripts/battery_monitor_node.py index 40ad02af..614bf110 100755 --- a/auv_control/auv_control/scripts/battery_monitor_node.py +++ b/auv_control/auv_control/scripts/battery_monitor_node.py @@ -3,17 +3,24 @@ import rospy from auv_msgs.msg import Power + class BatteryMonitorNode: def __init__(self): # Initialize the node - rospy.init_node('battery_monitor_node', anonymous=True) + rospy.init_node("battery_monitor_node", anonymous=True) # Parameters - self.voltage_threshold = rospy.get_param('~minimum_voltage_threshold', 13.0) - self.voltage_warn_threshold = rospy.get_param('~voltage_warn_threshold', 15.0) - self.timeout_duration = rospy.get_param('~power_message_timeout', 5) # Timeout duration in seconds - self.min_print_interval = rospy.get_param('~min_log_interval', 1.0) # Minimum print interval in seconds - self.max_print_interval = rospy.get_param('~max_log_interval', 10.0) # Maximum print interval in seconds + self.voltage_threshold = rospy.get_param("~minimum_voltage_threshold", 13.0) + self.voltage_warn_threshold = rospy.get_param("~voltage_warn_threshold", 15.0) + self.timeout_duration = rospy.get_param( + "~power_message_timeout", 5 + ) # Timeout duration in seconds + self.min_print_interval = rospy.get_param( + "~min_log_interval", 1.0 + ) # Minimum print interval in seconds + self.max_print_interval = rospy.get_param( + "~max_log_interval", 10.0 + ) # Maximum print interval in seconds # Variables self.last_msg_time = rospy.Time.now() @@ -23,13 +30,21 @@ def __init__(self): self.print_interval = 1.0 # Start with a slow print interval (e.g., 1 seconds) # Subscribers - self.power_sub = rospy.Subscriber('power', Power, self.power_callback) + self.power_sub = rospy.Subscriber("power", Power, self.power_callback) # Timers - self.voltage_print_timer = rospy.Timer(rospy.Duration(self.print_interval), self.print_voltage_callback, oneshot=True) + self.voltage_print_timer = rospy.Timer( + rospy.Duration(self.print_interval), + self.print_voltage_callback, + oneshot=True, + ) # self.timeout_check_timer = rospy.Timer(rospy.Duration(0.1), self.check_timeout_and_low_voltage_callback) - rospy.loginfo("Power Monitor Node started with voltage threshold: {:.2f}".format(self.voltage_threshold)) + rospy.loginfo( + "Power Monitor Node started with voltage threshold: {:.2f}".format( + self.voltage_threshold + ) + ) def power_callback(self, msg): self.voltage = msg.voltage @@ -39,61 +54,95 @@ def power_callback(self, msg): def reset_timer_with_new_interval(self, new_interval): self.print_interval = new_interval self.voltage_print_timer.shutdown() - self.voltage_print_timer = rospy.Timer(rospy.Duration(self.print_interval), self.print_voltage_callback, oneshot=True) + self.voltage_print_timer = rospy.Timer( + rospy.Duration(self.print_interval), + self.print_voltage_callback, + oneshot=True, + ) def print_voltage(self): - + if self.voltage == None: self.reset_timer_with_new_interval(1.0) rospy.logwarn("No voltage received.") return - - self.is_timeouted = rospy.Time.now() - self.last_msg_time > rospy.Duration(self.timeout_duration) + + self.is_timeouted = rospy.Time.now() - self.last_msg_time > rospy.Duration( + self.timeout_duration + ) if self.is_timeouted: self.reset_timer_with_new_interval(1.0) - rospy.logerr("Power message timeout. Last voltage: {:.2f} V".format(self.voltage)) + rospy.logerr( + "Power message timeout. Last voltage: {:.2f} V".format(self.voltage) + ) return - + if self.voltage < self.voltage_threshold: self.reset_timer_with_new_interval(1.0) - rospy.logerr("Battery voltage is below {:.2f}V threshold: {:.2f} V".format(self.voltage_threshold, self.voltage)) + rospy.logerr( + "Battery voltage is below {:.2f}V threshold: {:.2f} V".format( + self.voltage_threshold, self.voltage + ) + ) return elif self.voltage < self.voltage_warn_threshold: self.reset_timer_with_new_interval(1.0) - rospy.loginfo("Battery voltage:" + "\033[93m" + " {:.2f} V".format(self.voltage) + "\033[0m") + rospy.loginfo( + "Battery voltage:" + + "\033[93m" + + " {:.2f} V".format(self.voltage) + + "\033[0m" + ) return elif self.voltage >= self.voltage_warn_threshold: new_interval = self.interpolate_print_interval(self.voltage) self.reset_timer_with_new_interval(new_interval) - rospy.loginfo("Battery voltage:" + "\033[92m" + " {:.2f} V".format(self.voltage) + "\033[0m") + rospy.loginfo( + "Battery voltage:" + + "\033[92m" + + " {:.2f} V".format(self.voltage) + + "\033[0m" + ) return - + # if still alive, set the timer for the next print self.print_interval = 1.0 self.voltage_print_timer.shutdown() - self.voltage_print_timer = rospy.Timer(rospy.Duration(self.print_interval), self.print_voltage_callback, oneshot=True) - + self.voltage_print_timer = rospy.Timer( + rospy.Duration(self.print_interval), + self.print_voltage_callback, + oneshot=True, + ) + def print_voltage_callback(self, event): self.print_voltage() def check_timeout_and_low_voltage_callback(self, event): - new_timeout = rospy.Time.now() - self.last_msg_time > rospy.Duration(self.timeout_duration) + new_timeout = rospy.Time.now() - self.last_msg_time > rospy.Duration( + self.timeout_duration + ) self.is_undervoltage = self.voltage < self.voltage_threshold - + if new_timeout != self.is_timeouted and self.voltage != None: if not self.is_undervoltage: self.print_voltage() - self.is_timeouted = new_timeout - + self.is_timeouted = new_timeout + if self.is_timeouted: if self.voltage != None: - rospy.logerr("Power message timeout. Last voltage: {:.2f} V".format(self.voltage)) + rospy.logerr( + "Power message timeout. Last voltage: {:.2f} V".format(self.voltage) + ) else: rospy.logerr("Power message timeout. No voltage received.") return - + if self.voltage != None and self.is_undervoltage: - rospy.logerr("Battery voltage is below {:.2f}V threshold: {:.2f} V".format(self.voltage_threshold, self.voltage)) + rospy.logerr( + "Battery voltage is below {:.2f}V threshold: {:.2f} V".format( + self.voltage_threshold, self.voltage + ) + ) def interpolate_print_interval(self, voltage): # Interpolate between 5 seconds and 60 seconds based on voltage @@ -108,12 +157,17 @@ def interpolate_print_interval(self, voltage): return max_interval else: # Linear interpolation - return (voltage - min_voltage) / (max_voltage - min_voltage) * (max_interval - min_interval) + return ( + (voltage - min_voltage) + / (max_voltage - min_voltage) + * (max_interval - min_interval) + ) def spin(self): rospy.spin() -if __name__ == '__main__': + +if __name__ == "__main__": try: node = BatteryMonitorNode() node.spin() diff --git a/auv_control/auv_control/scripts/prop_transform_publisher.py b/auv_control/auv_control/scripts/prop_transform_publisher.py index 647e5a25..97b2fc14 100755 --- a/auv_control/auv_control/scripts/prop_transform_publisher.py +++ b/auv_control/auv_control/scripts/prop_transform_publisher.py @@ -4,7 +4,13 @@ import math import tf import message_filters -from geometry_msgs.msg import PoseStamped, PoseArray, Pose, TransformStamped, PointStamped +from geometry_msgs.msg import ( + PoseStamped, + PoseArray, + Pose, + TransformStamped, + PointStamped, +) from std_msgs.msg import Float32 from auv_msgs.srv import SetObjectTransform, SetObjectTransformRequest from ultralytics_ros.msg import YoloResult @@ -17,27 +23,30 @@ import copy import threading + class Scene: def __init__(self): self.lock = threading.Lock() self.objects = {} - + def add_object_to_location(self, id: int, x: float, y: float, z: float): with self.lock: if id not in self.objects: self.objects[id] = [] - + # check if the new location belongs to already existing object or a new object, just by checking the distance for obj in self.objects[id]: - distance = math.sqrt((obj.x - x)**2 + (obj.y - y)**2 + (obj.z - z)**2) + distance = math.sqrt( + (obj.x - x) ** 2 + (obj.y - y) ** 2 + (obj.z - z) ** 2 + ) if distance < 4.0: obj.update_position(x, y, z) return - + # if it is octagon, dont add new octagon if there is already one if id == 14 and len(self.objects[id]) > 0: - pass # don't add new octagon - else: + pass # don't add new octagon + else: new_object = SceneObject(id, x, y, z) self.objects[id].append(new_object) @@ -45,7 +54,7 @@ def get_objects(self): objects_copy = None with self.lock: objects_copy = copy.deepcopy(self.objects) - + return objects_copy def update_objects(self): @@ -53,12 +62,13 @@ def update_objects(self): for key, value in self.objects.items(): for obj in value: obj.update_filtered_position() - + def print_all_objects_and_their_count(self): with self.lock: for key, value in self.objects.items(): print(f"Object {id_to_name_map[key]}: {len(value)}") + class SceneObject: def __init__(self, id: int, x: float, y: float, z: float): self.id = id @@ -68,21 +78,22 @@ def __init__(self, id: int, x: float, y: float, z: float): self.filtered_x = x self.filtered_y = y self.filtered_z = z - + def get_position(self): return self.x, self.y, self.z - + def update_filtered_position(self): alpha = 0.2 self.filtered_x = alpha * self.x + (1 - alpha) * self.filtered_x self.filtered_y = alpha * self.y + (1 - alpha) * self.filtered_y self.filtered_z = alpha * self.z + (1 - alpha) * self.filtered_z - + def update_position(self, x: float, y: float, z: float): self.x = x self.y = y self.z = z + name_to_id_map = { "red_buoy": 8, "gate_red_arrow": 4, @@ -90,7 +101,7 @@ def update_position(self, x: float, y: float, z: float): "gate_middle_part": 5, "torpedo_map": 12, "bin_whole": 9, - "octagon": 14 + "octagon": 14, } id_to_name_map = { @@ -100,13 +111,15 @@ def update_position(self, x: float, y: float, z: float): 5: "gate_middle_part", 12: "torpedo_map", 9: "bin_whole", - 14: "octagon" + 14: "octagon", } + class CameraCalibration: def __init__(self, namespace: str): self.calibration = camera_calibrations.CameraCalibrationFetcher( - namespace, True).get_camera_info() + namespace, True + ).get_camera_info() def calculate_angles(self, pixel_coordinates: tuple) -> tuple: fx = self.calibration.K[0] @@ -118,17 +131,18 @@ def calculate_angles(self, pixel_coordinates: tuple) -> tuple: angle_x = math.atan(norm_x) angle_y = math.atan(norm_y) return angle_x, angle_y - + def distance_from_height(self, real_height: float, measured_height: float) -> float: focal_length = self.calibration.K[4] distance = (real_height * focal_length) / measured_height return distance - + def distance_from_width(self, real_width: float, measured_width: float) -> float: focal_length = self.calibration.K[0] distance = (real_width * focal_length) / measured_width return distance + class Prop: def __init__(self, id: int, name: str, real_height: float, real_width: float): self.id = id @@ -137,18 +151,27 @@ def __init__(self, id: int, name: str, real_height: float, real_width: float): self.real_width = real_width self.scene_objects = {} - def estimate_distance(self, measured_height: float, measured_width: float, calibration: CameraCalibration): - + def estimate_distance( + self, + measured_height: float, + measured_width: float, + calibration: CameraCalibration, + ): + distance_from_height = None distance_from_width = None distance_avarage = None - + if self.real_height is not None: - distance_from_height = calibration.distance_from_height(self.real_height, measured_height) - + distance_from_height = calibration.distance_from_height( + self.real_height, measured_height + ) + if self.real_width is not None: - distance_from_width = calibration.distance_from_width(self.real_width, measured_width) - + distance_from_width = calibration.distance_from_width( + self.real_width, measured_width + ) + if distance_from_height is not None and distance_from_width is not None: distance_avarage = (distance_from_height + distance_from_width) * 0.5 return distance_avarage @@ -160,10 +183,12 @@ def estimate_distance(self, measured_height: float, measured_width: float, calib rospy.logerr(f"Could not estimate distance for prop {self.name}") return None + class GateRedArrow(Prop): def __init__(self): super().__init__(4, "gate_red_arrow", 0.3048, 0.3048) + class GateBlueArrow(Prop): def __init__(self): super().__init__(3, "gate_blue_arrow", 0.3048, 0.3048) @@ -193,6 +218,7 @@ class Octagon(Prop): def __init__(self): super().__init__(14, "octagon", 0.92, 1.30) + class ObjectPositionEstimator: def __init__(self): rospy.init_node("object_position_estimator", anonymous=True) @@ -201,7 +227,7 @@ def __init__(self): self.camera_calibrations = { "taluy/cameras/cam_front": CameraCalibration("taluy/cameras/cam_front"), - "taluy/cameras/cam_bottom": CameraCalibration("taluy/cameras/cam_bottom") + "taluy/cameras/cam_bottom": CameraCalibration("taluy/cameras/cam_bottom"), } self.camera_frames = { @@ -216,11 +242,23 @@ def __init__(self): "gate_middle_part": GateMiddlePart(), "torpedo_map": TorpedoMap(), # "bin_whole": BinWhole(), - "octagon": Octagon() + "octagon": Octagon(), } self.id_tf_map = { - "taluy/cameras/cam_front": {8: "red_buoy", 7: "path", 9: "bin_whole", 12: "torpedo_map", 13: "torpedo_hole", 1: "gate_left", 2: "gate_right", 3: "gate_blue_arrow", 4: "gate_red_arrow", 5: "gate_middle_part", 14: "octagon"}, + "taluy/cameras/cam_front": { + 8: "red_buoy", + 7: "path", + 9: "bin_whole", + 12: "torpedo_map", + 13: "torpedo_hole", + 1: "gate_left", + 2: "gate_right", + 3: "gate_blue_arrow", + 4: "gate_red_arrow", + 5: "gate_middle_part", + 14: "octagon", + }, "taluy/cameras/cam_bottom": {9: "bin/whole", 10: "bin/red", 11: "bin/blue"}, } @@ -247,19 +285,20 @@ def __init__(self): # Initialize PoseArray publisher self.detection_line_pubs = { x: rospy.Publisher( - f"/taluy/missions/{x}/detection_lines", PoseArray, queue_size=10) + f"/taluy/missions/{x}/detection_lines", PoseArray, queue_size=10 + ) for x in self.id_tf_map["taluy/cameras/cam_front"].values() } # create an area publisher for each detection self.detection_distance_pubs = { x: rospy.Publisher( - f"/taluy/missions/{x}/detection_distance", Float32, queue_size=10) + f"/taluy/missions/{x}/detection_distance", Float32, queue_size=10 + ) for x in self.id_tf_map["taluy/cameras/cam_front"].values() } - self.pose_array_pub = rospy.Publisher( - '/line_topic', PoseArray, queue_size=10) + self.pose_array_pub = rospy.Publisher("/line_topic", PoseArray, queue_size=10) # Initialize tf2 buffer and listener self.tf_buffer = tf2_ros.Buffer() @@ -273,8 +312,7 @@ def __init__(self): self.set_object_transform_service.wait_for_service() # Subscriptions - yolo_result_subscriber = message_filters.Subscriber( - "/yolo_result", YoloResult) + yolo_result_subscriber = message_filters.Subscriber("/yolo_result", YoloResult) altitude_subscriber = message_filters.Subscriber( "/taluy/sensors/dvl/altitude", Float32 ) @@ -298,14 +336,14 @@ def __init__(self): def scene_transform_publisher_callback(self, event): # first update scene objects self.scene.update_objects() - + # traverse over all objects in the scene and publish their transform as tf for key, value in self.scene.get_objects().items(): # find out the closest object to the taluy/base_link closest_object = None min_distance = 1000.0 for obj in value: - + point1 = PointStamped() point1.header.frame_id = "odom" point1.point.x = obj.filtered_x @@ -313,7 +351,9 @@ def scene_transform_publisher_callback(self, event): point1.point.z = obj.filtered_z transform = None try: - transform = self.tf_buffer.lookup_transform("taluy/base_link", "odom", rospy.Time(0), rospy.Duration(1.0)) + transform = self.tf_buffer.lookup_transform( + "taluy/base_link", "odom", rospy.Time(0), rospy.Duration(1.0) + ) except ( tf2_ros.LookupException, tf2_ros.ConnectivityException, @@ -321,14 +361,18 @@ def scene_transform_publisher_callback(self, event): ): rospy.logerr("Error looking up transform") return - + point1_odom = tf2_geometry_msgs.do_transform_point(point1, transform) - - distance = math.sqrt(point1_odom.point.x**2 + point1_odom.point.y**2 + point1_odom.point.z**2) + + distance = math.sqrt( + point1_odom.point.x**2 + + point1_odom.point.y**2 + + point1_odom.point.z**2 + ) if distance < min_distance: min_distance = distance closest_object = obj - + for obj in value: transform_message = TransformStamped() transform_message.header.stamp = rospy.Time.now() @@ -336,7 +380,9 @@ def scene_transform_publisher_callback(self, event): if obj == closest_object: transform_message.child_frame_id = f"{id_to_name_map[key]}_link" else: - transform_message.child_frame_id = f"{id_to_name_map[key]}_{value.index(obj)}_link" + transform_message.child_frame_id = ( + f"{id_to_name_map[key]}_{value.index(obj)}_link" + ) transform_message.transform.translation.x = obj.filtered_x transform_message.transform.translation.y = obj.filtered_y transform_message.transform.translation.z = obj.filtered_z @@ -344,10 +390,9 @@ def scene_transform_publisher_callback(self, event): transform_message.transform.rotation.y = 0.0 transform_message.transform.rotation.z = 0.0 transform_message.transform.rotation.w = 1.0 - + self.broadcaster.sendTransform(transform_message) - - + # def dvl_callback(self, msg: Float32): # self.latest_altitude = msg.data @@ -365,20 +410,26 @@ def scene_transform_publisher_callback(self, event): # distance = (real_width * focal_length) / measured_width # return distance - def check_if_detection_is_inside_image(self, detection, image_width: int = 640, image_height: int = 480) -> bool: + def check_if_detection_is_inside_image( + self, detection, image_width: int = 640, image_height: int = 480 + ) -> bool: center = detection.bbox.center half_size_x = detection.bbox.size_x * 0.5 half_size_y = detection.bbox.size_y * 0.5 deadzone = 5 # pixels - if center.x + half_size_x >= image_width-deadzone or center.x - half_size_x <= deadzone: + if ( + center.x + half_size_x >= image_width - deadzone + or center.x - half_size_x <= deadzone + ): return False - if center.y + half_size_y >= image_height-deadzone or center.y - half_size_y <= deadzone: + if ( + center.y + half_size_y >= image_height - deadzone + or center.y - half_size_y <= deadzone + ): return False return True - def callback( - self, detection_msg: YoloResult, altitude_msg: Float32 - ): + def callback(self, detection_msg: YoloResult, altitude_msg: Float32): print("Received messages") for detection in detection_msg.detections.detections: if len(detection.results) == 0: @@ -390,26 +441,31 @@ def callback( if detection_id in self.id_tf_map["taluy/cameras/cam_front"]: detection_name = self.id_tf_map["taluy/cameras/cam_front"][detection_id] - + if detection_id == 9: self.process_altitude_projection(detection, altitude) continue - + if detection_name in self.props: prop = self.props[detection_name] measured_height = detection.bbox.size_y measured_width = detection.bbox.size_x if self.check_if_detection_is_inside_image(detection) == False: continue - distance = prop.estimate_distance(measured_height, measured_width, self.camera_calibrations["taluy/cameras/cam_front"]) + distance = prop.estimate_distance( + measured_height, + measured_width, + self.camera_calibrations["taluy/cameras/cam_front"], + ) if distance is not None: detection_distance = Float32() detection_distance.data = distance self.detection_distance_pubs[detection_name].publish( - detection_distance) + detection_distance + ) self.process_front_estimated_camera( - detection, distance, suffix="average") - + detection, distance, suffix="average" + ) # if detection_id in self.id_tf_map["taluy/cameras/cam_front"]: @@ -438,11 +494,11 @@ def callback( # self.process_front_estimated_camera( # detection, distance_avarage, suffix="average") - # if id is bin whole - # if detection_id == 9: - # self.process_altitude_projection(detection, altitude) + # if id is bin whole + # if detection_id == 9: + # self.process_altitude_projection(detection, altitude) - # self.process_front_camera(detection, 15.0) + # self.process_front_camera(detection, 15.0) # if detection_id in self.id_tf_map["taluy/cameras/cam_bottom"] and altitude is not None: # self.process_bottom_camera(detection, altitude) @@ -456,11 +512,17 @@ def transform_pose_to_odom(self, pose: Pose, source_frame: str) -> Pose: try: transform = self.tf_buffer.lookup_transform( - "odom", source_frame, rospy.Time(0), rospy.Duration(1000000.0)) + "odom", source_frame, rospy.Time(0), rospy.Duration(1000000.0) + ) transformed_pose_stamped = tf2_geometry_msgs.do_transform_pose( - pose_stamped, transform) + pose_stamped, transform + ) return transformed_pose_stamped.pose - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + except ( + tf2_ros.LookupException, + tf2_ros.ConnectivityException, + tf2_ros.ExtrapolationException, + ) as e: rospy.logerr(f"Error transforming pose: {e}") return None @@ -470,7 +532,8 @@ def process_front_camera(self, detection, distance: float): detection_name = self.id_tf_map[camera_name][detection_id] angle_x, angle_y = self.camera_calibrations[camera_name].calculate_angles( - (detection.bbox.center.x, detection.bbox.center.y)) + (detection.bbox.center.x, detection.bbox.center.y) + ) # angle_x, angle_y = calculate_angles( # self.camera_calibrations[camera_name].calibration, @@ -510,9 +573,11 @@ def process_front_camera(self, detection, distance: float): # Transform poses to the odom frame start_pose_odom = self.transform_pose_to_odom( - start_pose, self.camera_frames[camera_name]) + start_pose, self.camera_frames[camera_name] + ) end_pose_odom = self.transform_pose_to_odom( - end_pose, self.camera_frames[camera_name]) + end_pose, self.camera_frames[camera_name] + ) if start_pose_odom and end_pose_odom: pose_array.poses.append(start_pose_odom) @@ -534,7 +599,8 @@ def process_bottom_camera(self, detection, distance: float): detection_id = detection.results[0].id angle_x, angle_y = self.camera_calibrations[camera_name].calculate_angles( - (detection.bbox.center.x, detection.bbox.center.y)) + (detection.bbox.center.x, detection.bbox.center.y) + ) # angle_x, angle_y = calculate_angles( # self.camera_calibrations[camera_name].calibration, @@ -548,7 +614,9 @@ def process_bottom_camera(self, detection, distance: float): transform_message = TransformStamped() transform_message.header.stamp = rospy.Time.now() transform_message.header.frame_id = self.camera_frames[camera_name] - transform_message.child_frame_id = f"{self.id_tf_map[camera_name][detection_id]}_link" + transform_message.child_frame_id = ( + f"{self.id_tf_map[camera_name][detection_id]}_link" + ) transform_message.transform.translation.x = offset_x transform_message.transform.translation.y = offset_y transform_message.transform.translation.z = distance @@ -559,12 +627,15 @@ def process_bottom_camera(self, detection, distance: float): self.send_transform(transform_message) - def process_front_estimated_camera(self, detection, distance: float, suffix: str = ""): + def process_front_estimated_camera( + self, detection, distance: float, suffix: str = "" + ): camera_name = "taluy/cameras/cam_front" detection_id = detection.results[0].id angle_x, angle_y = self.camera_calibrations[camera_name].calculate_angles( - (detection.bbox.center.x, detection.bbox.center.y)) + (detection.bbox.center.x, detection.bbox.center.y) + ) # angle_x, angle_y = calculate_angles( # self.camera_calibrations[camera_name].calibration, @@ -580,22 +651,33 @@ def process_front_estimated_camera(self, detection, distance: float, suffix: str point1.point.x = offset_x point1.point.y = offset_y point1.point.z = distance - + try: # transform from optical_camera_frame to odom frame transform = self.tf_buffer.lookup_transform( - "odom", "taluy/base_link/front_camera_optical_link", rospy.Time(0), rospy.Duration(1.0)) + "odom", + "taluy/base_link/front_camera_optical_link", + rospy.Time(0), + rospy.Duration(1.0), + ) point1_odom = tf2_geometry_msgs.do_transform_point(point1, transform) - self.scene.add_object_to_location(detection_id, point1_odom.point.x, point1_odom.point.y, point1_odom.point.z) + self.scene.add_object_to_location( + detection_id, + point1_odom.point.x, + point1_odom.point.y, + point1_odom.point.z, + ) self.scene.print_all_objects_and_their_count() - + except tf2_ros.LookupException as e: rospy.logerr(f"Error transforming point: {e}") transform_message = TransformStamped() transform_message.header.stamp = rospy.Time.now() transform_message.header.frame_id = self.camera_frames[camera_name] - transform_message.child_frame_id = f"{self.id_tf_map[camera_name][detection_id]}_link" + transform_message.child_frame_id = ( + f"{self.id_tf_map[camera_name][detection_id]}_link" + ) transform_message.transform.translation.x = offset_x transform_message.transform.translation.y = offset_y transform_message.transform.translation.z = distance @@ -609,21 +691,21 @@ def process_front_estimated_camera(self, detection, distance: float, suffix: str def calculate_intersection_with_ground(self, point1_odom, point2_odom): # Calculate t where the z component is zero (ground plane) if point2_odom.point.z != point1_odom.point.z: - t = -point1_odom.point.z / \ - (point2_odom.point.z - point1_odom.point.z) + t = -point1_odom.point.z / (point2_odom.point.z - point1_odom.point.z) # Check if t is within the segment range [0, 1] if 0 <= t <= 1: # Calculate intersection point - x = point1_odom.point.x + t * \ - (point2_odom.point.x - point1_odom.point.x) - y = point1_odom.point.y + t * \ - (point2_odom.point.y - point1_odom.point.y) + x = point1_odom.point.x + t * ( + point2_odom.point.x - point1_odom.point.x + ) + y = point1_odom.point.y + t * ( + point2_odom.point.y - point1_odom.point.y + ) z = 0 # ground plane return x, y, z else: - rospy.logwarn( - "No intersection with ground plane within the segment.") + rospy.logwarn("No intersection with ground plane within the segment.") return None else: rospy.logwarn("The line segment is parallel to the ground plane.") @@ -637,7 +719,8 @@ def process_altitude_projection(self, detection, altitude: float): bbox_bottom_y = detection.bbox.center.y + detection.bbox.size_y * 0.5 angle_x, angle_y = self.camera_calibrations[camera_name].calculate_angles( - (bbox_bottom_x, bbox_bottom_y)) + (bbox_bottom_x, bbox_bottom_y) + ) # angle_x, angle_y = calculate_angles( # self.camera_calibrations[camera_name].calibration, @@ -665,11 +748,13 @@ def process_altitude_projection(self, detection, altitude: float): try: # optical_camera_frame'den odom frame'ine transform transform = self.tf_buffer.lookup_transform( - "odom", "taluy/base_link/front_camera_optical_link", rospy.Time(0), rospy.Duration(1000000.0)) - point1_odom = tf2_geometry_msgs.do_transform_point( - point1, transform) - point2_odom = tf2_geometry_msgs.do_transform_point( - point2, transform) + "odom", + "taluy/base_link/front_camera_optical_link", + rospy.Time(0), + rospy.Duration(1000000.0), + ) + point1_odom = tf2_geometry_msgs.do_transform_point(point1, transform) + point2_odom = tf2_geometry_msgs.do_transform_point(point2, transform) except tf2_ros.LookupException as e: rospy.logerr(f"Error transforming point: {e}") @@ -679,11 +764,12 @@ def process_altitude_projection(self, detection, altitude: float): # find the intersection of the line from point1 to point2 to the plane z=0 # find the xyz here, where z=0 intersect_return = self.calculate_intersection_with_ground( - point1_odom, point2_odom) + point1_odom, point2_odom + ) if not intersect_return: return - + x_, y_, z_ = intersect_return self.scene.add_object_to_location(detection_id, x_, y_, z_) diff --git a/auv_control/auv_control/src/controller_ros.cpp b/auv_control/auv_control/src/controller_ros.cpp index 46de8bc3..40b3efa9 100644 --- a/auv_control/auv_control/src/controller_ros.cpp +++ b/auv_control/auv_control/src/controller_ros.cpp @@ -1 +1 @@ -#include "auv_control/controller_ros.h" \ No newline at end of file +#include "auv_control/controller_ros.h" diff --git a/auv_control/auv_control/src/thruster_manager_ros.cpp b/auv_control/auv_control/src/thruster_manager_ros.cpp index f05086c2..dbfc89f1 100644 --- a/auv_control/auv_control/src/thruster_manager_ros.cpp +++ b/auv_control/auv_control/src/thruster_manager_ros.cpp @@ -1 +1 @@ -#include "auv_control/thruster_manager_ros.h" \ No newline at end of file +#include "auv_control/thruster_manager_ros.h" diff --git a/auv_control/auv_controllers/CMakeLists.txt b/auv_control/auv_controllers/CMakeLists.txt index 0c18f4ff..ffb44846 100644 --- a/auv_control/auv_controllers/CMakeLists.txt +++ b/auv_control/auv_controllers/CMakeLists.txt @@ -45,13 +45,13 @@ install(DIRECTORY include/${PROJECT_NAME}/ ## Testing ## ############# -catkin_add_gtest(${PROJECT_NAME}-test +catkin_add_gtest(${PROJECT_NAME}-test test/test_multidof_pid_controller/test_multidof_pid_controller.cpp test/main.cpp ) if(TARGET ${PROJECT_NAME}-test) -target_link_libraries(${PROJECT_NAME}-test +target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} ${catkin_LIBRARIES} ) diff --git a/auv_control/auv_controllers/controller_plugins.xml b/auv_control/auv_controllers/controller_plugins.xml index 294c96f5..5757c3c7 100644 --- a/auv_control/auv_controllers/controller_plugins.xml +++ b/auv_control/auv_controllers/controller_plugins.xml @@ -1,6 +1,6 @@ + - + 6DOF PID Controller Plugin - \ No newline at end of file + diff --git a/auv_control/auv_controllers/include/auv_controllers/controller_base.h b/auv_control/auv_controllers/include/auv_controllers/controller_base.h index 03137619..df323153 100644 --- a/auv_control/auv_controllers/include/auv_controllers/controller_base.h +++ b/auv_control/auv_controllers/include/auv_controllers/controller_base.h @@ -41,4 +41,4 @@ class ControllerBase { using SixDOFControllerBase = ControllerBase<6>; } // namespace control -} // namespace auv \ No newline at end of file +} // namespace auv diff --git a/auv_control/auv_controllers/include/auv_controllers/model.h b/auv_control/auv_controllers/include/auv_controllers/model.h index 28032b6c..fba9bd01 100644 --- a/auv_control/auv_controllers/include/auv_controllers/model.h +++ b/auv_control/auv_controllers/include/auv_controllers/model.h @@ -60,4 +60,4 @@ inline std::ostream& operator<<(std::ostream& os, const Model& model) { using SixDOFModel = Model<6>; } // namespace control -} // namespace auv \ No newline at end of file +} // namespace auv diff --git a/auv_control/auv_controllers/include/auv_controllers/multidof_pid_controller.h b/auv_control/auv_controllers/include/auv_controllers/multidof_pid_controller.h index ea6fd6f4..11d7e9bc 100644 --- a/auv_control/auv_controllers/include/auv_controllers/multidof_pid_controller.h +++ b/auv_control/auv_controllers/include/auv_controllers/multidof_pid_controller.h @@ -131,4 +131,4 @@ class MultiDOFPIDController : public ControllerBase { using SixDOFPIDController = MultiDOFPIDController<6>; } // namespace control -} // namespace auv \ No newline at end of file +} // namespace auv diff --git a/auv_control/auv_controllers/package.xml b/auv_control/auv_controllers/package.xml index 815e96a0..38e6a1ec 100644 --- a/auv_control/auv_controllers/package.xml +++ b/auv_control/auv_controllers/package.xml @@ -3,19 +3,15 @@ auv_controllers 0.1.0 The auv_controllers package - Sencer Yazici Sencer Yazici - BSD-3-Clause - catkin roscpp roscpp roscpp auv_common_lib - - + - \ No newline at end of file + diff --git a/auv_control/auv_controllers/src/model.cpp b/auv_control/auv_controllers/src/model.cpp index 9d9639c9..ae6b046d 100644 --- a/auv_control/auv_controllers/src/model.cpp +++ b/auv_control/auv_controllers/src/model.cpp @@ -29,4 +29,4 @@ auv::control::Model<6> parse(const XmlRpc::XmlRpcValue& param) { return model; }; -} // namespace auv::common::rosparam::detail \ No newline at end of file +} // namespace auv::common::rosparam::detail diff --git a/auv_control/auv_controllers/src/multidof_pid_controller.cpp b/auv_control/auv_controllers/src/multidof_pid_controller.cpp index 0fef1e3d..7a1d7fa0 100644 --- a/auv_control/auv_controllers/src/multidof_pid_controller.cpp +++ b/auv_control/auv_controllers/src/multidof_pid_controller.cpp @@ -3,4 +3,4 @@ #include PLUGINLIB_EXPORT_CLASS(auv::control::SixDOFPIDController, - auv::control::SixDOFControllerBase) \ No newline at end of file + auv::control::SixDOFControllerBase) diff --git a/auv_control/auv_controllers/test/main.cpp b/auv_control/auv_controllers/test/main.cpp index 54cef320..d3e13cee 100644 --- a/auv_control/auv_controllers/test/main.cpp +++ b/auv_control/auv_controllers/test/main.cpp @@ -3,4 +3,4 @@ int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} diff --git a/auv_control/auv_controllers/test/test_multidof_pid_controller/test_multidof_pid_controller.cpp b/auv_control/auv_controllers/test/test_multidof_pid_controller/test_multidof_pid_controller.cpp index d2c59534..e6a47a6d 100644 --- a/auv_control/auv_controllers/test/test_multidof_pid_controller/test_multidof_pid_controller.cpp +++ b/auv_control/auv_controllers/test/test_multidof_pid_controller/test_multidof_pid_controller.cpp @@ -42,4 +42,4 @@ TEST(MultiDOFPIDController, TestVelocityControlOnly) { EXPECT_NEAR(force_torque(3), expected_force, kEpsilon); EXPECT_NEAR(force_torque(4), expected_force, kEpsilon); EXPECT_NEAR(force_torque(5), expected_force, kEpsilon); -} \ No newline at end of file +} diff --git a/auv_description/auv_common_descriptions/meshes/bar30/bar30.dae b/auv_description/auv_common_descriptions/meshes/bar30/bar30.dae index 54ec950f..37b7eb60 100644 --- a/auv_description/auv_common_descriptions/meshes/bar30/bar30.dae +++ b/auv_description/auv_common_descriptions/meshes/bar30/bar30.dae @@ -56,4 +56,4 @@ - \ No newline at end of file + diff --git a/auv_description/auv_common_descriptions/meshes/ping360/ping360.dae b/auv_description/auv_common_descriptions/meshes/ping360/ping360.dae index 43db3edc..ee0500b4 100644 --- a/auv_description/auv_common_descriptions/meshes/ping360/ping360.dae +++ b/auv_description/auv_common_descriptions/meshes/ping360/ping360.dae @@ -56,4 +56,4 @@ - \ No newline at end of file + diff --git a/auv_description/auv_common_descriptions/meshes/sonar/sonar.dae b/auv_description/auv_common_descriptions/meshes/sonar/sonar.dae index 36f76d35..6e7c2f53 100644 --- a/auv_description/auv_common_descriptions/meshes/sonar/sonar.dae +++ b/auv_description/auv_common_descriptions/meshes/sonar/sonar.dae @@ -56,4 +56,4 @@ - \ No newline at end of file + diff --git a/auv_description/auv_common_descriptions/meshes/t200/propeller.dae b/auv_description/auv_common_descriptions/meshes/t200/propeller.dae index d2cfca61..e02092aa 100644 --- a/auv_description/auv_common_descriptions/meshes/t200/propeller.dae +++ b/auv_description/auv_common_descriptions/meshes/t200/propeller.dae @@ -385,4 +385,4 @@ - \ No newline at end of file + diff --git a/auv_description/auv_common_descriptions/meshes/t200/shell.dae b/auv_description/auv_common_descriptions/meshes/t200/shell.dae index 2aa231d2..7bd2f8c0 100644 --- a/auv_description/auv_common_descriptions/meshes/t200/shell.dae +++ b/auv_description/auv_common_descriptions/meshes/t200/shell.dae @@ -96,4 +96,4 @@ - \ No newline at end of file + diff --git a/auv_description/auv_common_descriptions/meshes/wayfinder_dvl/dvl.dae b/auv_description/auv_common_descriptions/meshes/wayfinder_dvl/dvl.dae index 314a78ef..40900bb6 100644 --- a/auv_description/auv_common_descriptions/meshes/wayfinder_dvl/dvl.dae +++ b/auv_description/auv_common_descriptions/meshes/wayfinder_dvl/dvl.dae @@ -56,4 +56,4 @@ - \ No newline at end of file + diff --git a/auv_description/auv_common_descriptions/meshes/xsens_mti_g710/xsens_mti_g710.dae b/auv_description/auv_common_descriptions/meshes/xsens_mti_g710/xsens_mti_g710.dae index 279c4372..0802ca04 100644 --- a/auv_description/auv_common_descriptions/meshes/xsens_mti_g710/xsens_mti_g710.dae +++ b/auv_description/auv_common_descriptions/meshes/xsens_mti_g710/xsens_mti_g710.dae @@ -56,4 +56,4 @@ - \ No newline at end of file + diff --git a/auv_description/auv_common_descriptions/package.xml b/auv_description/auv_common_descriptions/package.xml index a83c8602..cf3e9246 100644 --- a/auv_description/auv_common_descriptions/package.xml +++ b/auv_description/auv_common_descriptions/package.xml @@ -4,15 +4,11 @@ 0.1.0 Package for containing common xacro description files used for building robots in URDF or Xacro. - Sencer Yazici Sencer Yazici - BSD-3-Clause - catkin xacro - diff --git a/auv_description/auv_common_descriptions/urdf/actuators.urdf.xacro b/auv_description/auv_common_descriptions/urdf/actuators.urdf.xacro index 19732e64..011f740d 100644 --- a/auv_description/auv_common_descriptions/urdf/actuators.urdf.xacro +++ b/auv_description/auv_common_descriptions/urdf/actuators.urdf.xacro @@ -1,4 +1,5 @@ + - - + + diff --git a/auv_description/auv_common_descriptions/urdf/actuators/t200.urdf.xacro b/auv_description/auv_common_descriptions/urdf/actuators/t200.urdf.xacro index dc8c199e..3d2ca08e 100644 --- a/auv_description/auv_common_descriptions/urdf/actuators/t200.urdf.xacro +++ b/auv_description/auv_common_descriptions/urdf/actuators/t200.urdf.xacro @@ -1,73 +1,61 @@ - - - + - + - + - + - + - - + + - - + - + - + - + - - + + - - - - - + + + + - - - - - - - + + + + + + - Gazebo/Orange - Gazebo/Orange - - \ No newline at end of file + diff --git a/auv_description/auv_common_descriptions/urdf/actuators/torpedo.urdf.xacro b/auv_description/auv_common_descriptions/urdf/actuators/torpedo.urdf.xacro index cd7170d0..1756dd4c 100644 --- a/auv_description/auv_common_descriptions/urdf/actuators/torpedo.urdf.xacro +++ b/auv_description/auv_common_descriptions/urdf/actuators/torpedo.urdf.xacro @@ -1,68 +1,60 @@ - - - + - - + - + - + - + - + - - + + - - + - + - + - + - - + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - \ No newline at end of file + diff --git a/auv_description/auv_common_descriptions/urdf/materials.urdf.xacro b/auv_description/auv_common_descriptions/urdf/materials.urdf.xacro index 7392f983..637edc36 100644 --- a/auv_description/auv_common_descriptions/urdf/materials.urdf.xacro +++ b/auv_description/auv_common_descriptions/urdf/materials.urdf.xacro @@ -1,22 +1,18 @@ - + - - + - - + - - + - - + diff --git a/auv_description/auv_common_descriptions/urdf/sensors.urdf.xacro b/auv_description/auv_common_descriptions/urdf/sensors.urdf.xacro index dc6b3a79..881b5824 100644 --- a/auv_description/auv_common_descriptions/urdf/sensors.urdf.xacro +++ b/auv_description/auv_common_descriptions/urdf/sensors.urdf.xacro @@ -1,9 +1,10 @@ + - - - - - - - - \ No newline at end of file + + + + + + + + diff --git a/auv_description/auv_common_descriptions/urdf/sensors/bar30.urdf.xacro b/auv_description/auv_common_descriptions/urdf/sensors/bar30.urdf.xacro index aa27c6a0..26b7440a 100644 --- a/auv_description/auv_common_descriptions/urdf/sensors/bar30.urdf.xacro +++ b/auv_description/auv_common_descriptions/urdf/sensors/bar30.urdf.xacro @@ -1,43 +1,36 @@ - + - + - + - + - + - - + + - - - + - - - - + + + + - - - - - + + + + - \ No newline at end of file + diff --git a/auv_description/auv_common_descriptions/urdf/sensors/h2c_hydrophone.urdf.xacro b/auv_description/auv_common_descriptions/urdf/sensors/h2c_hydrophone.urdf.xacro index 78c7ee85..78773ddc 100644 --- a/auv_description/auv_common_descriptions/urdf/sensors/h2c_hydrophone.urdf.xacro +++ b/auv_description/auv_common_descriptions/urdf/sensors/h2c_hydrophone.urdf.xacro @@ -1,29 +1,29 @@ - + - + - + - + - + - - + + - - - - + + + + - \ No newline at end of file + diff --git a/auv_description/auv_common_descriptions/urdf/sensors/logitech_c920_camera.urdf.xacro b/auv_description/auv_common_descriptions/urdf/sensors/logitech_c920_camera.urdf.xacro index 221b5970..12a59642 100644 --- a/auv_description/auv_common_descriptions/urdf/sensors/logitech_c920_camera.urdf.xacro +++ b/auv_description/auv_common_descriptions/urdf/sensors/logitech_c920_camera.urdf.xacro @@ -1,46 +1,42 @@ - - + + - + - + - + - + - - + + - - - + - - - - + + + + - - - - - + + + + - \ No newline at end of file + diff --git a/auv_description/auv_common_descriptions/urdf/sensors/ping360.urdf.xacro b/auv_description/auv_common_descriptions/urdf/sensors/ping360.urdf.xacro index ba9ca495..2af34ccf 100644 --- a/auv_description/auv_common_descriptions/urdf/sensors/ping360.urdf.xacro +++ b/auv_description/auv_common_descriptions/urdf/sensors/ping360.urdf.xacro @@ -1,43 +1,36 @@ - + - + - + - + - + - - + + - - - + - - - - + + + + - - - - - + + + + - \ No newline at end of file + diff --git a/auv_description/auv_common_descriptions/urdf/sensors/sonar.urdf.xacro b/auv_description/auv_common_descriptions/urdf/sensors/sonar.urdf.xacro index abfb61df..c055c9bf 100644 --- a/auv_description/auv_common_descriptions/urdf/sensors/sonar.urdf.xacro +++ b/auv_description/auv_common_descriptions/urdf/sensors/sonar.urdf.xacro @@ -1,34 +1,29 @@ - + - + - + - + - + - - + + - - - - - + + + + diff --git a/auv_description/auv_common_descriptions/urdf/sensors/wayfinder_dvl.urdf.xacro b/auv_description/auv_common_descriptions/urdf/sensors/wayfinder_dvl.urdf.xacro index a50cc715..8604366d 100644 --- a/auv_description/auv_common_descriptions/urdf/sensors/wayfinder_dvl.urdf.xacro +++ b/auv_description/auv_common_descriptions/urdf/sensors/wayfinder_dvl.urdf.xacro @@ -1,65 +1,57 @@ - + - + - - + + - + - - + + - - + - + - - - + + - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - \ No newline at end of file + diff --git a/auv_description/auv_common_descriptions/urdf/sensors/xsens_mti_g710.urdf.xacro b/auv_description/auv_common_descriptions/urdf/sensors/xsens_mti_g710.urdf.xacro index 255635f3..67e92d84 100644 --- a/auv_description/auv_common_descriptions/urdf/sensors/xsens_mti_g710.urdf.xacro +++ b/auv_description/auv_common_descriptions/urdf/sensors/xsens_mti_g710.urdf.xacro @@ -1,34 +1,29 @@ - + - + - + - + - + - - + + - - - - - + + + + - \ No newline at end of file + diff --git a/auv_description/auv_description/CMakeLists.txt b/auv_description/auv_description/CMakeLists.txt index 9e32d554..2ea5f526 100644 --- a/auv_description/auv_description/CMakeLists.txt +++ b/auv_description/auv_description/CMakeLists.txt @@ -8,4 +8,4 @@ catkin_package() install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) \ No newline at end of file +) diff --git a/auv_description/auv_description/launch/start_state_publisher.launch b/auv_description/auv_description/launch/start_state_publisher.launch index 9ea2c2bd..647b3f19 100644 --- a/auv_description/auv_description/launch/start_state_publisher.launch +++ b/auv_description/auv_description/launch/start_state_publisher.launch @@ -1,25 +1,25 @@ - + - + - + - + - + - + - \ No newline at end of file + diff --git a/auv_description/auv_description/package.xml b/auv_description/auv_description/package.xml index 5b46c216..df6996be 100644 --- a/auv_description/auv_description/package.xml +++ b/auv_description/auv_description/package.xml @@ -3,19 +3,15 @@ auv_description 0.1.0 The meta-package containing all the description files for the auv robots. - Sencer Yazici Sencer Yazici - BSD-3-Clause - catkin auv_common_descriptions taluy_description robot_state_publisher joint_state_publisher - - \ No newline at end of file + diff --git a/auv_description/taluy_description/package.xml b/auv_description/taluy_description/package.xml index 93080b5b..33636969 100644 --- a/auv_description/taluy_description/package.xml +++ b/auv_description/taluy_description/package.xml @@ -3,15 +3,11 @@ taluy_description 0.1.0 The taluy_description package - Sencer Yazici Sencer Yazici - BSD-3-Clause - catkin auv_common_descriptions - diff --git a/auv_description/taluy_description/urdf/auv.urdf.xacro b/auv_description/taluy_description/urdf/auv.urdf.xacro index cb403ae0..33c5b4bd 100644 --- a/auv_description/taluy_description/urdf/auv.urdf.xacro +++ b/auv_description/taluy_description/urdf/auv.urdf.xacro @@ -1,5 +1,5 @@ - - - + + + diff --git a/auv_description/taluy_description/urdf/auv_base.urdf.xacro b/auv_description/taluy_description/urdf/auv_base.urdf.xacro index 243353f5..03d9f8ec 100644 --- a/auv_description/taluy_description/urdf/auv_base.urdf.xacro +++ b/auv_description/taluy_description/urdf/auv_base.urdf.xacro @@ -1,130 +1,60 @@ - + - - - - - - + + + + - - - + + + - - + - + - + - - + - + - - - + - - - + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + - \ No newline at end of file + diff --git a/auv_hardware/auv_hardware/package.xml b/auv_hardware/auv_hardware/package.xml index dde5c952..c430a9fa 100644 --- a/auv_hardware/auv_hardware/package.xml +++ b/auv_hardware/auv_hardware/package.xml @@ -3,15 +3,11 @@ auv_hardware 0.1.0 Metapackage for hardware interfacing - Sencer Yazici Sencer Yazici - BSD-3-Clause - catkin auv_hardware_bridge - diff --git a/auv_hardware/auv_hardware_bridge/CMakeLists.txt b/auv_hardware/auv_hardware_bridge/CMakeLists.txt index 93766aed..f4f5f919 100644 --- a/auv_hardware/auv_hardware_bridge/CMakeLists.txt +++ b/auv_hardware/auv_hardware_bridge/CMakeLists.txt @@ -20,7 +20,7 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -add_executable(serial_to_ros_bridge_node +add_executable(serial_to_ros_bridge_node src/serial_to_ros_bridge_node.cpp src/serial_to_ros_bridge.cpp ) diff --git a/auv_hardware/auv_hardware_bridge/include/auv_hardware_bridge/serial_to_ros_bridge.hpp b/auv_hardware/auv_hardware_bridge/include/auv_hardware_bridge/serial_to_ros_bridge.hpp index 7a7d10ff..1011dbe8 100644 --- a/auv_hardware/auv_hardware_bridge/include/auv_hardware_bridge/serial_to_ros_bridge.hpp +++ b/auv_hardware/auv_hardware_bridge/include/auv_hardware_bridge/serial_to_ros_bridge.hpp @@ -9,12 +9,12 @@ namespace auv_hardware { class SerialToROSBridge { -public: + public: SerialToROSBridge(ros::NodeHandle &nh); void spin(); -private: + private: void input_data_callback(const std_msgs::UInt8MultiArray::ConstPtr &msg); void handle_incoming_messages(); @@ -29,4 +29,4 @@ class SerialToROSBridge { char buffer_[1024]; }; -}; // namespace auv_hardware +}; // namespace auv_hardware diff --git a/auv_hardware/auv_hardware_bridge/package.xml b/auv_hardware/auv_hardware_bridge/package.xml index eb2114da..54537535 100644 --- a/auv_hardware/auv_hardware_bridge/package.xml +++ b/auv_hardware/auv_hardware_bridge/package.xml @@ -3,16 +3,12 @@ auv_hardware_bridge 0.1.0 Contains bridge nodes for interfacing with hardware - Sencer Yazici Sencer Yazici - BSD-3-Clause - catkin roscpp rosserial_python - diff --git a/auv_msgs/msg/MotorCommand.msg b/auv_msgs/msg/MotorCommand.msg index cc7ceb6c..7bf9ad81 100644 --- a/auv_msgs/msg/MotorCommand.msg +++ b/auv_msgs/msg/MotorCommand.msg @@ -1 +1 @@ -uint16[8] channels # pulse between 1100 - 1900 us \ No newline at end of file +uint16[8] channels # pulse between 1100 - 1900 us diff --git a/auv_msgs/package.xml b/auv_msgs/package.xml index 3f63600f..80e7202f 100644 --- a/auv_msgs/package.xml +++ b/auv_msgs/package.xml @@ -3,11 +3,9 @@ auv_msgs 0.1.0 The auv_msgs package - Sencer Yazici Sencer Yazici BSD-3-Clause - catkin geometry_msgs actionlib_msgs @@ -18,11 +16,8 @@ geometry_msgs nav_msgs actionlib_msgs - - - - \ No newline at end of file + diff --git a/auv_msgs/srv/AlignFrameController.srv b/auv_msgs/srv/AlignFrameController.srv index 5c451c05..5a406e29 100644 --- a/auv_msgs/srv/AlignFrameController.srv +++ b/auv_msgs/srv/AlignFrameController.srv @@ -3,4 +3,4 @@ string target_frame float32 angle_offset --- bool success -string message \ No newline at end of file +string message diff --git a/auv_msgs/srv/SetDepth.srv b/auv_msgs/srv/SetDepth.srv index 82b9310b..f2d65177 100644 --- a/auv_msgs/srv/SetDepth.srv +++ b/auv_msgs/srv/SetDepth.srv @@ -1,4 +1,4 @@ float32 target_depth --- bool success -string message \ No newline at end of file +string message diff --git a/auv_navigation/auv_localization/CMakeLists.txt b/auv_navigation/auv_localization/CMakeLists.txt index 91837657..a2371d40 100644 --- a/auv_navigation/auv_localization/CMakeLists.txt +++ b/auv_navigation/auv_localization/CMakeLists.txt @@ -39,8 +39,8 @@ catkin_install_python(PROGRAMS DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(DIRECTORY - launch +install(DIRECTORY + launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/auv_navigation/auv_localization/config/imu_calibration_data.yaml b/auv_navigation/auv_localization/config/imu_calibration_data.yaml index 4393223e..369f8688 100644 --- a/auv_navigation/auv_localization/config/imu_calibration_data.yaml +++ b/auv_navigation/auv_localization/config/imu_calibration_data.yaml @@ -1,4 +1,5 @@ +--- drift: -- 0.0040029836201003815 -- -0.0015402579327184883 -- 0.010440763762785799 + - 0.0040029836201003815 + - -0.0015402579327184883 + - 0.010440763762785799 diff --git a/auv_navigation/auv_localization/config/sensor_calibration_data.yaml b/auv_navigation/auv_localization/config/sensor_calibration_data.yaml index 17263263..8cb95944 100644 --- a/auv_navigation/auv_localization/config/sensor_calibration_data.yaml +++ b/auv_navigation/auv_localization/config/sensor_calibration_data.yaml @@ -1,3 +1,4 @@ +--- sensors/external_pressure_sensor: depth_covariance: 0.05 depth_offset: -0.36411 diff --git a/auv_navigation/auv_localization/launch/start.launch b/auv_navigation/auv_localization/launch/start.launch index f4e4aa19..7e872843 100644 --- a/auv_navigation/auv_localization/launch/start.launch +++ b/auv_navigation/auv_localization/launch/start.launch @@ -23,4 +23,4 @@ - \ No newline at end of file + diff --git a/auv_navigation/auv_localization/package.xml b/auv_navigation/auv_localization/package.xml index 8172f44b..397b6fd5 100644 --- a/auv_navigation/auv_localization/package.xml +++ b/auv_navigation/auv_localization/package.xml @@ -3,21 +3,17 @@ auv_localization 0.1.0 The auv_localization package - Batuhan Ozer Batuhan Ozer BSD-3-Clause - message_generation std_msgs message_runtime std_msgs - catkin roscpp auv_common_lib auv_controllers nav_msgs auv_msgs - - \ No newline at end of file + diff --git a/auv_navigation/auv_localization/scripts/dvl_odometry_node.py b/auv_navigation/auv_localization/scripts/dvl_odometry_node.py index be29507c..a9b07a01 100644 --- a/auv_navigation/auv_localization/scripts/dvl_odometry_node.py +++ b/auv_navigation/auv_localization/scripts/dvl_odometry_node.py @@ -11,12 +11,13 @@ import time from auv_common_lib.logging.terminal_color_utils import TerminalColors + class DvlToOdom: def __init__(self): - rospy.init_node('dvl_to_odom_node', anonymous=True) + rospy.init_node("dvl_to_odom_node", anonymous=True) # Parameters for first-order filter - self.cmdvel_tau = rospy.get_param('~cmdvel_tau', 0.1) + self.cmdvel_tau = rospy.get_param("~cmdvel_tau", 0.1) self.linear_x_covariance = rospy.get_param( "sensors/dvl/covariance/linear_x", 0.000015 ) @@ -26,21 +27,34 @@ def __init__(self): self.linear_z_covariance = rospy.get_param( "sensors/dvl/covariance/linear_z", 0.00005 ) - + # Subscribers and Publishers - self.dvl_velocity_subscriber = message_filters.Subscriber("sensors/dvl/velocity_raw", Twist) - self.is_valid_subscriber = message_filters.Subscriber("sensors/dvl/is_valid", Bool) - self.cmd_vel_subscriber = rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback) - - self.sync = message_filters.ApproximateTimeSynchronizer([self.dvl_velocity_subscriber, self.is_valid_subscriber], queue_size=10, slop=0.1, allow_headerless=True) + self.dvl_velocity_subscriber = message_filters.Subscriber( + "sensors/dvl/velocity_raw", Twist + ) + self.is_valid_subscriber = message_filters.Subscriber( + "sensors/dvl/is_valid", Bool + ) + self.cmd_vel_subscriber = rospy.Subscriber( + "cmd_vel", Twist, self.cmd_vel_callback + ) + + self.sync = message_filters.ApproximateTimeSynchronizer( + [self.dvl_velocity_subscriber, self.is_valid_subscriber], + queue_size=10, + slop=0.1, + allow_headerless=True, + ) self.sync.registerCallback(self.dvl_callback) - self.odom_publisher = rospy.Publisher("localization/odom_dvl", Odometry, queue_size=10) + self.odom_publisher = rospy.Publisher( + "localization/odom_dvl", Odometry, queue_size=10 + ) # Initialize the odometry message self.odom_msg = Odometry() - self.odom_msg.header.frame_id = 'odom' - self.odom_msg.child_frame_id = 'taluy/base_link' + self.odom_msg.header.frame_id = "odom" + self.odom_msg.child_frame_id = "taluy/base_link" # Initialize covariances with default values self.odom_msg.pose.covariance = np.zeros(36).tolist() @@ -49,14 +63,22 @@ def __init__(self): self.odom_msg.twist.covariance[0] = self.linear_x_covariance self.odom_msg.twist.covariance[7] = self.linear_y_covariance self.odom_msg.twist.covariance[14] = self.linear_z_covariance - + # Log loaded parameters - DVL_odometry_colored = TerminalColors.color_text("DVL Odometry Calibration data loaded", TerminalColors.PASTEL_BLUE) + DVL_odometry_colored = TerminalColors.color_text( + "DVL Odometry Calibration data loaded", TerminalColors.PASTEL_BLUE + ) rospy.loginfo(f"{DVL_odometry_colored} : cmdvel_tau: {self.cmdvel_tau}") - rospy.loginfo(f"{DVL_odometry_colored} : linear x covariance: {self.linear_x_covariance}") - rospy.loginfo(f"{DVL_odometry_colored} : linear y covariance: {self.linear_y_covariance}") - rospy.loginfo(f"{DVL_odometry_colored} : linear z covariance: {self.linear_z_covariance}") - + rospy.loginfo( + f"{DVL_odometry_colored} : linear x covariance: {self.linear_x_covariance}" + ) + rospy.loginfo( + f"{DVL_odometry_colored} : linear y covariance: {self.linear_y_covariance}" + ) + rospy.loginfo( + f"{DVL_odometry_colored} : linear z covariance: {self.linear_z_covariance}" + ) + # Initialize variables for fallback mechanism self.cmd_vel_twist = Twist() self.filtered_cmd_vel = Twist() @@ -64,11 +86,13 @@ def __init__(self): def transform_vector(self, vector): theta = np.radians(-135) - rotation_matrix = np.array([ - [np.cos(theta), -np.sin(theta), 0], - [np.sin(theta), np.cos(theta), 0], - [0, 0, 1] - ]) + rotation_matrix = np.array( + [ + [np.cos(theta), -np.sin(theta), 0], + [np.sin(theta), np.cos(theta), 0], + [0, 0, 1], + ] + ) return np.dot(rotation_matrix, np.array(vector)) def cmd_vel_callback(self, cmd_vel_msg): @@ -79,12 +103,30 @@ def filter_cmd_vel(self): dt = (current_time - self.last_update_time).to_sec() self.alpha = dt / (self.cmdvel_tau + dt) - self.filtered_cmd_vel.linear.x = self.filtered_cmd_vel.linear.x * (1.0 - self.alpha) + self.alpha * self.cmd_vel_twist.linear.x - self.filtered_cmd_vel.linear.y = self.filtered_cmd_vel.linear.y * (1.0 - self.alpha) + self.alpha * self.cmd_vel_twist.linear.y - self.filtered_cmd_vel.linear.z = self.filtered_cmd_vel.linear.z * (1.0 - self.alpha) + self.alpha * self.cmd_vel_twist.linear.z - self.filtered_cmd_vel.angular.x = self.filtered_cmd_vel.angular.x * (1.0 - self.alpha) + self.alpha * self.cmd_vel_twist.angular.x - self.filtered_cmd_vel.angular.y = self.filtered_cmd_vel.angular.y * (1.0 - self.alpha) + self.alpha * self.cmd_vel_twist.angular.y - self.filtered_cmd_vel.angular.z = self.filtered_cmd_vel.angular.z * (1.0 - self.alpha) + self.alpha * self.cmd_vel_twist.angular.z + self.filtered_cmd_vel.linear.x = ( + self.filtered_cmd_vel.linear.x * (1.0 - self.alpha) + + self.alpha * self.cmd_vel_twist.linear.x + ) + self.filtered_cmd_vel.linear.y = ( + self.filtered_cmd_vel.linear.y * (1.0 - self.alpha) + + self.alpha * self.cmd_vel_twist.linear.y + ) + self.filtered_cmd_vel.linear.z = ( + self.filtered_cmd_vel.linear.z * (1.0 - self.alpha) + + self.alpha * self.cmd_vel_twist.linear.z + ) + self.filtered_cmd_vel.angular.x = ( + self.filtered_cmd_vel.angular.x * (1.0 - self.alpha) + + self.alpha * self.cmd_vel_twist.angular.x + ) + self.filtered_cmd_vel.angular.y = ( + self.filtered_cmd_vel.angular.y * (1.0 - self.alpha) + + self.alpha * self.cmd_vel_twist.angular.y + ) + self.filtered_cmd_vel.angular.z = ( + self.filtered_cmd_vel.angular.z * (1.0 - self.alpha) + + self.alpha * self.cmd_vel_twist.angular.z + ) self.last_update_time = current_time @@ -92,7 +134,9 @@ def dvl_callback(self, velocity_msg, is_valid_msg): self.filter_cmd_vel() # Determine which data to use for odometry based on DVL validity if is_valid_msg.data: - rotated_vector = self.transform_vector([velocity_msg.linear.x, velocity_msg.linear.y, velocity_msg.linear.z]) + rotated_vector = self.transform_vector( + [velocity_msg.linear.x, velocity_msg.linear.y, velocity_msg.linear.z] + ) velocity_msg.linear.x = rotated_vector[0] velocity_msg.linear.y = rotated_vector[1] velocity_msg.linear.z = rotated_vector[2] @@ -101,7 +145,7 @@ def dvl_callback(self, velocity_msg, is_valid_msg): velocity_msg.linear.x = self.filtered_cmd_vel.linear.x velocity_msg.linear.y = self.filtered_cmd_vel.linear.y velocity_msg.linear.z = self.filtered_cmd_vel.linear.z - + # Fill the odometry message self.odom_msg.header.stamp = rospy.Time.now() self.odom_msg.twist.twist = velocity_msg @@ -118,7 +162,7 @@ def run(self): rospy.spin() -if __name__ == '__main__': +if __name__ == "__main__": try: dvl_to_odom = DvlToOdom() dvl_to_odom.run() diff --git a/auv_navigation/auv_localization/scripts/imu_odometry_node.py b/auv_navigation/auv_localization/scripts/imu_odometry_node.py index 4cd474a6..fcd157f5 100644 --- a/auv_navigation/auv_localization/scripts/imu_odometry_node.py +++ b/auv_navigation/auv_localization/scripts/imu_odometry_node.py @@ -12,20 +12,23 @@ class ImuToOdom: def __init__(self): - rospy.init_node('imu_to_odom_node', anonymous=True) + rospy.init_node("imu_to_odom_node", anonymous=True) self.imu_calibration_data_path = rospy.get_param( - '~imu_calibration_path', 'config/imu_calibration_data.yaml') + "~imu_calibration_path", "config/imu_calibration_data.yaml" + ) # Subscribers and Publishers self.imu_subscriber = rospy.Subscriber( - "sensors/imu/data", Imu, self.imu_callback) + "sensors/imu/data", Imu, self.imu_callback + ) self.odom_publisher = rospy.Publisher( - "localization/odom_imu", Odometry, queue_size=10) + "localization/odom_imu", Odometry, queue_size=10 + ) # Initialize the odometry message self.odom_msg = Odometry() - self.odom_msg.header.frame_id = 'odom' - self.odom_msg.child_frame_id = 'taluy/base_link' + self.odom_msg.header.frame_id = "odom" + self.odom_msg.child_frame_id = "taluy/base_link" # Initialize covariances with zeros self.odom_msg.pose.covariance = np.zeros(36).tolist() @@ -41,12 +44,18 @@ def __init__(self): # Service for IMU calibration self.calibration_service = rospy.Service( - 'calibrate_imu', CalibrateIMU, self.calibrate_imu) + "calibrate_imu", CalibrateIMU, self.calibrate_imu + ) def imu_callback(self, imu_msg): if self.calibrating: self.calibration_data.append( - [imu_msg.angular_velocity.x, imu_msg.angular_velocity.y, imu_msg.angular_velocity.z]) + [ + imu_msg.angular_velocity.x, + imu_msg.angular_velocity.y, + imu_msg.angular_velocity.z, + ] + ) # Fill the odometry message with orientation and angular velocity data from the IMU self.odom_msg.header.stamp = rospy.Time.now() @@ -54,19 +63,19 @@ def imu_callback(self, imu_msg): # Orientation self.odom_msg.pose.pose.orientation = imu_msg.orientation # Only the first 9 elements for orientation - self.odom_msg.pose.covariance[:9] = list( - imu_msg.orientation_covariance) + self.odom_msg.pose.covariance[:9] = list(imu_msg.orientation_covariance) # Angular Velocity corrected_angular_velocity = Vector3( imu_msg.angular_velocity.x - self.drift[0], imu_msg.angular_velocity.y - self.drift[1], - imu_msg.angular_velocity.z - self.drift[2] + imu_msg.angular_velocity.z - self.drift[2], ) self.odom_msg.twist.twist.angular = corrected_angular_velocity # Angular velocity covariance in the correct position self.odom_msg.twist.covariance[21:30] = list( - imu_msg.angular_velocity_covariance) + imu_msg.angular_velocity_covariance + ) # Set the position to zero as we are not computing it here self.odom_msg.pose.pose.position.x = 0.0 @@ -95,25 +104,30 @@ def calibrate_imu(self, req): self.drift = np.mean(self.calibration_data, axis=0) self.save_calibration_data() rospy.loginfo(f"IMU Calibration completed. Drift: {self.drift}") - return CalibrateIMUResponse(success=True, message="IMU Calibration successful") + return CalibrateIMUResponse( + success=True, message="IMU Calibration successful" + ) else: - return CalibrateIMUResponse(success=False, message="IMU Calibration failed, no data recorded") + return CalibrateIMUResponse( + success=False, message="IMU Calibration failed, no data recorded" + ) def save_calibration_data(self): - calibration_data = { - 'drift': self.drift.tolist() - } - with open(self.imu_calibration_data_path, 'w') as f: + calibration_data = {"drift": self.drift.tolist()} + with open(self.imu_calibration_data_path, "w") as f: yaml.dump(calibration_data, f) - rospy.loginfo(f"{TerminalColors.OKGREEN} Calibration data saved.{TerminalColors.ENDC}") + rospy.loginfo( + f"{TerminalColors.OKGREEN} Calibration data saved.{TerminalColors.ENDC}" + ) def load_calibration_data(self): try: - with open(self.imu_calibration_data_path, 'r') as f: + with open(self.imu_calibration_data_path, "r") as f: calibration_data = yaml.safe_load(f) - self.drift = np.array(calibration_data['drift']) + self.drift = np.array(calibration_data["drift"]) rospy.loginfo( - f"{TerminalColors.OKYELLOW}IMU Calibration data loaded.{TerminalColors.ENDC} Drift: {self.drift}") + f"{TerminalColors.OKYELLOW}IMU Calibration data loaded.{TerminalColors.ENDC} Drift: {self.drift}" + ) except FileNotFoundError: rospy.logerr("No calibration data found.") @@ -121,7 +135,7 @@ def run(self): rospy.spin() -if __name__ == '__main__': +if __name__ == "__main__": try: imu_to_odom = ImuToOdom() imu_to_odom.run() diff --git a/auv_navigation/auv_mapping/launch/start.launch b/auv_navigation/auv_mapping/launch/start.launch index cc64aa62..cf8dcd26 100644 --- a/auv_navigation/auv_mapping/launch/start.launch +++ b/auv_navigation/auv_mapping/launch/start.launch @@ -11,7 +11,7 @@ - + senceryazici - - TODO - - - - - - @@ -55,11 +46,8 @@ smach rospy smach - - - diff --git a/auv_smach/src/auv_smach/red_buoy.py b/auv_smach/src/auv_smach/red_buoy.py index 6f8f68d3..3cf3262d 100644 --- a/auv_smach/src/auv_smach/red_buoy.py +++ b/auv_smach/src/auv_smach/red_buoy.py @@ -44,14 +44,17 @@ def __init__( self.tf_broadcaster = tf2_ros.TransformBroadcaster() self.rate = rospy.Rate(10) - self.linear_velocity = 0.8 # rospy.get_param("/smach/max_linear_velocity") - self.angular_velocity = 0.8 # rospy.get_param("/smach/max_angular_velocity") + self.linear_velocity = 0.8 # rospy.get_param("/smach/max_linear_velocity") + self.angular_velocity = 0.8 # rospy.get_param("/smach/max_angular_velocity") def execute(self, userdata): try: # Lookup the initial transform from center_frame to base_frame center_to_base_transform = self.tf_buffer.lookup_transform( - self.center_frame, self.base_frame, rospy.Time(0), rospy.Duration(100000.0) + self.center_frame, + self.base_frame, + rospy.Time(0), + rospy.Duration(100000.0), ) # Calculate the initial angle from the center to the base @@ -135,7 +138,10 @@ def execute(self, userdata): try: # Lookup the transform from center_frame to base_frame center_to_base_transform = self.tf_buffer.lookup_transform( - self.center_frame, self.base_frame, rospy.Time(0), rospy.Duration(10000.0) + self.center_frame, + self.base_frame, + rospy.Time(0), + rospy.Duration(10000.0), ) # Calculate the position from the center to the base @@ -229,7 +235,10 @@ def execute(self, userdata): try: # Lookup the transform from base_frame to look_at_frame base_to_look_at_transform = self.tf_buffer.lookup_transform( - self.base_frame, self.look_at_frame, rospy.Time(0), rospy.Duration(10000.0) + self.base_frame, + self.look_at_frame, + rospy.Time(0), + rospy.Duration(10000.0), ) # Calculate the direction vector from base_frame to look_at_frame diff --git a/auv_software/package.xml b/auv_software/package.xml index c02afd65..a9c3583f 100644 --- a/auv_software/package.xml +++ b/auv_software/package.xml @@ -2,14 +2,10 @@ auv_software 0.1.0 - The core software package for the auv project. - Sencer Yazici Sencer Yazici - BSD-3-Clause - catkin auv_description auv_bringup @@ -17,7 +13,7 @@ auv_navigation auv_localization auv_vision - + diff --git a/auv_teleop/CMakeLists.txt b/auv_teleop/CMakeLists.txt index 906e9a06..7c1731d3 100644 --- a/auv_teleop/CMakeLists.txt +++ b/auv_teleop/CMakeLists.txt @@ -26,7 +26,7 @@ catkin_install_python(PROGRAMS DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(DIRECTORY - launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install(DIRECTORY + launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/auv_teleop/config/joy.yaml b/auv_teleop/config/joy.yaml index 0fbecbc8..02767ca2 100644 --- a/auv_teleop/config/joy.yaml +++ b/auv_teleop/config/joy.yaml @@ -1,3 +1,4 @@ +--- buttons: launch_torpedo1: 4 launch_torpedo2: 2 diff --git a/auv_teleop/config/ps5.yaml b/auv_teleop/config/ps5.yaml index 81f0473a..d4df2111 100644 --- a/auv_teleop/config/ps5.yaml +++ b/auv_teleop/config/ps5.yaml @@ -1,3 +1,4 @@ +--- buttons: launch_torpedo1: 0 launch_torpedo2: 2 diff --git a/auv_teleop/launch/start_teleop.launch b/auv_teleop/launch/start_teleop.launch index 9006b7e4..2c5cf4d1 100644 --- a/auv_teleop/launch/start_teleop.launch +++ b/auv_teleop/launch/start_teleop.launch @@ -3,7 +3,7 @@ - + @@ -15,4 +15,4 @@ - \ No newline at end of file + diff --git a/auv_teleop/package.xml b/auv_teleop/package.xml index ef627999..131d4812 100644 --- a/auv_teleop/package.xml +++ b/auv_teleop/package.xml @@ -3,11 +3,9 @@ auv_teleop 1.0.0 This package consists of scripts for teleoperating the vehicle. - Sencer Yazici BSD-3-Clause Sencer Yazici - catkin geometry_msgs joy @@ -21,10 +19,8 @@ joy rospy sensor_msgs - - diff --git a/auv_teleop/scripts/joy_manager.py b/auv_teleop/scripts/joy_manager.py index 4ba99707..b26ed912 100644 --- a/auv_teleop/scripts/joy_manager.py +++ b/auv_teleop/scripts/joy_manager.py @@ -7,6 +7,7 @@ import threading from std_srvs.srv import Trigger, TriggerRequest + class JoystickEvent: def __init__(self, change_threshold, callback): self.previous_value = 0.0 @@ -31,7 +32,7 @@ def __init__(self): self.publish_rate = 50 # 50 Hz self.rate = rospy.Rate(self.publish_rate) - + self.buttons = rospy.get_param("~buttons") self.axes = rospy.get_param("~axes") @@ -54,7 +55,6 @@ def __init__(self): self.joy_sub = rospy.Subscriber("joy", Joy, self.joy_callback) rospy.loginfo("Joystick node initialized") - def call_service_if_available(self, service, success_message, failure_message): try: service.wait_for_service(timeout=1) @@ -81,14 +81,19 @@ def drop_dropper(self): self.dropper_service, "Ball dropped", "Failed to drop the ball" ) - def joy_callback(self, msg): with self.lock: self.joy_data = msg - self.torpedo1_button_event.update(self.joy_data.buttons[self.buttons["launch_torpedo1"]]) - self.torpedo2_button_event.update(self.joy_data.buttons[self.buttons["launch_torpedo2"]]) - self.dropper_button_event.update(self.joy_data.buttons[self.buttons["drop_ball"]]) + self.torpedo1_button_event.update( + self.joy_data.buttons[self.buttons["launch_torpedo1"]] + ) + self.torpedo2_button_event.update( + self.joy_data.buttons[self.buttons["launch_torpedo2"]] + ) + self.dropper_button_event.update( + self.joy_data.buttons[self.buttons["drop_ball"]] + ) def run(self): while not rospy.is_shutdown(): @@ -99,13 +104,25 @@ def run(self): # Use axes with gain if self.joy_data.buttons[self.buttons["z_control"]]: twist.linear.x = 0.0 - twist.linear.z = self.joy_data.axes[self.axes["z_axis"]["index"]] * self.axes["z_axis"]["gain"] + twist.linear.z = ( + self.joy_data.axes[self.axes["z_axis"]["index"]] + * self.axes["z_axis"]["gain"] + ) else: - twist.linear.x = self.joy_data.axes[self.axes["x_axis"]["index"]] * self.axes["x_axis"]["gain"] + twist.linear.x = ( + self.joy_data.axes[self.axes["x_axis"]["index"]] + * self.axes["x_axis"]["gain"] + ) twist.linear.z = 0.0 - twist.linear.y = self.joy_data.axes[self.axes["y_axis"]["index"]] * self.axes["y_axis"]["gain"] - twist.angular.z = self.joy_data.axes[self.axes["yaw_axis"]["index"]] * self.axes["yaw_axis"]["gain"] + twist.linear.y = ( + self.joy_data.axes[self.axes["y_axis"]["index"]] + * self.axes["y_axis"]["gain"] + ) + twist.angular.z = ( + self.joy_data.axes[self.axes["yaw_axis"]["index"]] + * self.axes["yaw_axis"]["gain"] + ) else: twist.linear.x = 0.0 twist.angular.z = 0.0 diff --git a/docker/Dockerfile.auv-base b/docker/Dockerfile.auv-base index 7c190999..52a484d4 100644 --- a/docker/Dockerfile.auv-base +++ b/docker/Dockerfile.auv-base @@ -19,4 +19,3 @@ RUN apt-get update && \ ros-noetic-xacro \ ros-noetic-robot-localization && \ rm -rf /var/lib/apt/lists/* -