diff --git a/.github/workflows/run-tests.yml b/.github/workflows/run-tests.yml index 61b23c8..576240c 100644 --- a/.github/workflows/run-tests.yml +++ b/.github/workflows/run-tests.yml @@ -1,4 +1,4 @@ -# This workflow will run tests and linters with PyTest using Python 3.11 +# This workflow will run tests and linters using Python 3.11 # For more information see: https://docs.github.com/en/actions/about-github-actions name: Run linter and docker tests @@ -30,23 +30,9 @@ jobs: - name: Install tools run: | python -m pip install --upgrade pip - pip install black flake8 pylint pytest pytest-cov + pip install black # Run linters and formatters - name: Linters and formatters run: | black --check . - flake8 . --max-line-length=140 - pylint ros2_ws/src/slam/slam --disable=import-error,missing-module-docstring,missing-function-docstring,missing-class-docstring,broad-exception-caught,too-few-public-methods - - # Tests if Docker builds - - name: Docker build - run: docker build -f docker/Dockerfile -t temp-ci:latest docker/ - - - name: Colcon build (for the ros2 nodes) - run: | - docker run --rm \ - -v "$GITHUB_WORKSPACE/ros2_ws:/ros2_ws" \ - -w /ros2_ws \ - temp-ci:latest \ - bash -lc 'source /opt/ros/jazzy/setup.bash && colcon build --event-handlers console_cohesion+' \ No newline at end of file diff --git a/.gitignore b/.gitignore index bfd38ad..7bdf7bc 100755 --- a/.gitignore +++ b/.gitignore @@ -67,4 +67,7 @@ Icon? .com.apple.timemachine.donotpresent .com.apple.* -.venv/ \ No newline at end of file +venv/ + +.depthai_cached_models/ +.venv/ diff --git a/README.md b/README.md index 11eb709..cbfd8fd 100755 --- a/README.md +++ b/README.md @@ -1,54 +1 @@ -# Airside SLAM System (Docker) - -> [!NOTE] -> Please make sure to create a new branch instead of forking the repo when working on new tasks. - -The Docker setup provides: - -- ROS 2 Jazzy -- MAVROS -- Cartographer -- sf45b drivers - -## Build the image - -```sh -./docker/build.sh -``` - -## Run the container with GUI support - -```sh -# Run first terminal -sudo ./docker/run.sh - - -# Run more terminals -sudo ./docker/attach.sh -``` - -## Run SLAM commands - -```sh -cd /workspace/ros2_ws - -rm -rf build install log - -colcon build - -source install/setup.bash - -ros2 launch slam slam_launch.py - -# OR - -./sf45b - -ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 laser laser_frame - -ros2 run cartographer_ros cartographer_node -configuration_directory /workspace/ros2_ws/src/slam/config -configuration_basename sf45b_2d.lua --ros-args -p provide_odom_frame:=true -p expected_sensor_ids:="[scan]" -r scan:=/scan - -ros2 run cartographer_ros cartographer_occupancy_grid_node -resolution 0.05 -publish_period_sec 1.0 - -rviz2 -``` +# Airside SLAM System using OAK-D diff --git a/docker/Dockerfile b/docker/Dockerfile deleted file mode 100755 index de4bfa7..0000000 --- a/docker/Dockerfile +++ /dev/null @@ -1,116 +0,0 @@ -FROM ros:jazzy-ros-core - -ENV DEBIAN_FRONTEND=noninteractive -ENV ROS_DISTRO=jazzy - -RUN apt-get update && apt-get install -y --no-install-recommends \ - ros-jazzy-desktop \ - ros-jazzy-cartographer \ - ros-jazzy-cartographer-ros \ - ros-jazzy-cartographer-rviz \ - ros-jazzy-mavros \ - ros-jazzy-mavros-extras \ - cmake \ - python3-colcon-common-extensions \ - python3-rosdep \ - python3-vcstool \ - python3-pip \ - git \ - sudo \ - build-essential \ - curl \ - wget \ - gnupg \ - lsb-release \ - vim \ - tmux \ - less \ - net-tools \ - iputils-ping \ - xauth \ - x11-apps \ - xterm \ - mesa-utils \ - libgl1-mesa-dri \ - libglu1-mesa \ - libxkbcommon-x11-0 \ - python3-tk \ - geographiclib-tools \ - python3 \ - python3-venv \ - && rm -rf /var/lib/apt/lists/* - -RUN geographiclib-get-geoids best && geographiclib-get-gravity egm96 && geographiclib-get-magnetic emm2015 - -RUN rosdep init || true && rosdep update - -ARG USERNAME=root -ARG USER_UID=1000 -ARG USER_GID=1000 - -RUN set -eux; \ - if getent group "${USER_GID}" >/dev/null; then \ - EXISTING_GROUP_NAME="$(getent group ${USER_GID} | cut -d: -f1)"; \ - if [ "${EXISTING_GROUP_NAME}" != "${USERNAME}" ]; then \ - echo "GID ${USER_GID} taken by ${EXISTING_GROUP_NAME}; creating group ${USERNAME} with arbitrary GID"; \ - groupadd "${USERNAME}" || true; \ - fi; \ - else \ - groupadd -g "${USER_GID}" "${USERNAME}"; \ - fi; \ - if id -u "${USER_UID}" >/dev/null 2>&1; then \ - echo "UID ${USER_UID} in use; creating ${USERNAME} with next available UID"; \ - useradd -m -g "${USERNAME}" "${USERNAME}" || true; \ - else \ - useradd -m -u "${USER_UID}" -g "${USERNAME}" "${USERNAME}"; \ - fi; \ - for grp in dialout plugdev tty video; do \ - if getent group "$grp" > /dev/null 2>&1; then \ - usermod -a -G "$grp" "${USERNAME}" || true; \ - fi; \ - done; \ - mkdir -p /workspace/src; \ - chown -R "${USERNAME}:${USERNAME}" /workspace - -RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /root/.bashrc \ - && echo "[ -f /workspace/install/setup.bash ] && source /workspace/install/setup.bash" >> /root/.bashrc - -COPY --chown=${USERNAME}:${USERNAME} . /workspace - -RUN bash -lc 'if [ -d /workspace/ros2_ws/src ]; then cd /workspace/ros2_ws; colcon build || true; fi' - -RUN bash -lc "source /opt/ros/${ROS_DISTRO}/setup.bash && \ - git clone https://github.com/LightWare-Optoelectronics/lightwarelidar2 /opt/sf45-driver" - -ARG LIDAR_PORT=/dev/ttyAMA0 -ARG LIDAR_RANGE=320 - -RUN bash -lc "source /opt/ros/${ROS_DISTRO}/setup.bash && \ - cd /opt/sf45-driver && \ - sed -i 's|std::string portName = node->declare_parameter(\"port\", \"/dev/ttyUSB0\");|std::string portName = node->declare_parameter(\"port\", \"${LIDAR_PORT}\");|' src/sf45b.cpp && \ - HALF_RANGE=\$((${LIDAR_RANGE} / 2)) && \ - HIGH_ANGLE=\$HALF_RANGE && \ - LOW_ANGLE=\$((-\$HALF_RANGE)) && \ - sed -i \"s|params.highAngleLimit = node->declare_parameter(\\\"highAngleLimit\\\", 45.0f);|params.highAngleLimit = node->declare_parameter(\\\"highAngleLimit\\\", \$HIGH_ANGLE);|\" src/sf45b.cpp && \ - sed -i \"s|params.lowAngleLimit = node->declare_parameter(\\\"lowAngleLimit\\\", -45.0f);|params.lowAngleLimit = node->declare_parameter(\\\"lowAngleLimit\\\", \$LOW_ANGLE);|\" src/sf45b.cpp && \ - mkdir -p build && cd build && \ - cmake .. && cmake --build . --parallel $(nproc)" - -ENV QT_X11_NO_MITSHM=1 - -WORKDIR /workspace - -RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /root/.bashrc \ - && echo "[ -f /workspace/install/setup.bash ] && source /workspace/install/setup.bash" >> /root/.bashrc \ - && chown ${USERNAME}:${USERNAME} /root/.bashrc - -ENV VIRTUAL_ENV=/opt/venv -RUN python3 -m venv $VIRTUAL_ENV --system-site-packages -ENV PATH="$VIRTUAL_ENV/bin:$PATH" - -# Install dependencies: -RUN pip install pymavlink pyserial - -USER ${USERNAME} - -CMD ["bash"] diff --git a/docker/attach.sh b/docker/attach.sh deleted file mode 100755 index 6802784..0000000 --- a/docker/attach.sh +++ /dev/null @@ -1,6 +0,0 @@ -#!/usr/bin/env bash -set -e - -CONTAINER_NAME="ros-container" - -docker exec -it $CONTAINER_NAME bash -l diff --git a/docker/build.sh b/docker/build.sh deleted file mode 100755 index e022c4f..0000000 --- a/docker/build.sh +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env bash -set -e - -IMAGE_NAME="ros-ws" -LIDAR_PORT="${1:-/dev/ttyAMA0}" -LIDAR_RANGE="${2:-320}" - -docker build -t $IMAGE_NAME \ - --build-arg LIDAR_PORT="$LIDAR_PORT" \ - --build-arg LIDAR_RANGE="$LIDAR_RANGE" \ - -f docker/Dockerfile . diff --git a/docker/run.sh b/docker/run.sh deleted file mode 100755 index 3c3aee7..0000000 --- a/docker/run.sh +++ /dev/null @@ -1,62 +0,0 @@ -#!/usr/bin/env bash -set -e - -IMAGE_NAME="ros-ws" -CONTAINER_NAME="ros-container" - -if [ -z "${DISPLAY:-}" ]; then - if getent hosts host.docker.internal >/dev/null 2>&1; then - export DISPLAY=host.docker.internal:0 - else - export DISPLAY=:0 - fi -fi - -# Base Docker arguments -DOCKER_ARGS=( - -it - --name "$CONTAINER_NAME" - --rm - --ipc host - --pid host - --env QT_X11_NO_MITSHM=1 - --env DISPLAY - --network host - --volume "/tmp/.X11-unix:/tmp/.X11-unix:rw" - --volume "$(pwd):/workspace" -) - -SERIAL_GLOBS=(/dev/ttyAMA0 /dev/serial0 /dev/ttyS0 /dev/ttyUSB* /dev/ttyACM*) -for glob in "${SERIAL_GLOBS[@]}"; do - for dev in $glob; do - if [ -e "$dev" ]; then - DOCKER_ARGS+=(--device "$dev:$dev") - fi - done -done - -if [ -n "${SERIAL_DEVICES:-}" ]; then - for dev in ${SERIAL_DEVICES}; do - if [ -e "$dev" ]; then - DOCKER_ARGS+=(--device "$dev:$dev") - else - echo "[warn] Requested serial device $dev not found on host" >&2 - fi - done -fi - -echo "Launching container with serial devices passed through:" -printf ' %s\n' "${DOCKER_ARGS[@]}" | grep -- '--device' || echo " (none detected)" - -xhost +local:docker > /dev/null 2>&1 || true - -if docker ps -aq -f name=^${CONTAINER_NAME}$ > /dev/null 2>&1; then - if [[ -n "$(docker ps -aq -f name=^${CONTAINER_NAME}$)" ]]; then - docker rm -f "$CONTAINER_NAME" > /dev/null 2>&1 || true - fi -fi - -docker run "${DOCKER_ARGS[@]}" "$IMAGE_NAME" bash -lc "\ - cp /opt/sf45-driver/build/sf45b /workspace/sf45b 2>/dev/null || true; \ - chmod +x /workspace/sf45b 2>/dev/null || true; \ - exec bash" diff --git a/ros2_ws/src/slam/LICENSE b/ros2_ws/src/slam/LICENSE deleted file mode 100644 index d645695..0000000 --- a/ros2_ws/src/slam/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. diff --git a/ros2_ws/src/slam/config/sf45b_2d.lua b/ros2_ws/src/slam/config/sf45b_2d.lua deleted file mode 100644 index 39b1bfe..0000000 --- a/ros2_ws/src/slam/config/sf45b_2d.lua +++ /dev/null @@ -1,43 +0,0 @@ -include "map_builder.lua" -include "trajectory_builder.lua" - -options = { - map_builder = MAP_BUILDER, - trajectory_builder = TRAJECTORY_BUILDER, - map_frame = "map", - tracking_frame = "laser", -- "TODO: try laser_frame and see if the timestamp issue persists" - published_frame = "laser", -- "TODO: try laser_frame and see if the timestamp issue persists" - odom_frame = "odom", - provide_odom_frame = true, - publish_frame_projected_to_2d = false, - use_odometry = false, - use_nav_sat = false, - use_landmarks = false, - num_laser_scans = 1, - num_multi_echo_laser_scans = 0, - num_subdivisions_per_laser_scan = 1, - num_point_clouds = 0, - lookup_transform_timeout_sec = 0.2, - submap_publish_period_sec = 0.5, - pose_publish_period_sec = 5e-2, - trajectory_publish_period_sec = 30e-3, - rangefinder_sampling_ratio = 1.0, - odometry_sampling_ratio = 1.0, - fixed_frame_pose_sampling_ratio = 1.0, - imu_sampling_ratio = 1.0, - landmarks_sampling_ratio = 1.0, -} - -MAP_BUILDER.use_trajectory_builder_2d = true -TRAJECTORY_BUILDER_2D.min_range = 0.2 -TRAJECTORY_BUILDER_2D.max_range = 25.0 -TRAJECTORY_BUILDER_2D.missing_data_ray_length = 30.0 -TRAJECTORY_BUILDER_2D.use_imu_data = false -TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1 - -POSE_GRAPH.optimization_problem.huber_scale = 1e1 -POSE_GRAPH.optimize_every_n_nodes = 90 -POSE_GRAPH.constraint_builder.min_score = 0.55 -POSE_GRAPH.constraint_builder.global_localization_min_score = 0.6 - -return options diff --git a/ros2_ws/src/slam/launch/slam_launch.py b/ros2_ws/src/slam/launch/slam_launch.py deleted file mode 100644 index 3c7ebf6..0000000 --- a/ros2_ws/src/slam/launch/slam_launch.py +++ /dev/null @@ -1,106 +0,0 @@ -#!/usr/bin/env python3 - -from launch import LaunchDescription -from launch.actions import ExecuteProcess, DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - provide_odom_frame_arg = DeclareLaunchArgument( - "provide_odom_frame", - default_value="false", - description="Whether to provide odom frame", - ) - - expected_sensor_ids_arg = DeclareLaunchArgument( - "expected_sensor_ids", - default_value='["scan"]', - description="Expected sensor IDs for cartographer", - ) - - resolution_arg = DeclareLaunchArgument( - "resolution", - default_value="0.05", - description="Resolution for occupancy grid", - ) - - publish_period_arg = DeclareLaunchArgument( - "publish_period_sec", - default_value="1.0", - description="Publish period for occupancy grid", - ) - - slam_package_dir = FindPackageShare("slam") - - config_dir = PathJoinSubstitution([slam_package_dir, "config"]) - - # SF45B driver process - sf45b_process = ExecuteProcess( - cmd=["./sf45b"], cwd="/workspace", name="sf45b_driver", output="screen" - ) - - # Static transform publisher node - static_transform_publisher = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="laser_transform_publisher", - arguments=["0", "0", "0", "0", "0", "0", "laser", "laser_frame"], - output="screen", - ) - - # Cartographer node - cartographer_node = Node( - package="cartographer_ros", - executable="cartographer_node", - name="cartographer_node", - parameters=[ - {"provide_odom_frame": LaunchConfiguration("provide_odom_frame")}, - {"expected_sensor_ids": LaunchConfiguration("expected_sensor_ids")}, - ], - arguments=[ - "-configuration_directory", - config_dir, - "-configuration_basename", - "sf45b_2d.lua", - ], - remappings=[("scan", "/scan")], - output="screen", - ) - - # Cartographer occupancy grid node - occupancy_grid_node = Node( - package="cartographer_ros", - executable="cartographer_occupancy_grid_node", - name="cartographer_occupancy_grid_node", - arguments=[ - "-resolution", - LaunchConfiguration("resolution"), - "-publish_period_sec", - LaunchConfiguration("publish_period_sec"), - ], - output="screen", - ) - - # RViz2 node - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="screen", - ) - - return LaunchDescription( - [ - provide_odom_frame_arg, - expected_sensor_ids_arg, - resolution_arg, - publish_period_arg, - sf45b_process, - static_transform_publisher, - cartographer_node, - occupancy_grid_node, - rviz_node, - ] - ) diff --git a/ros2_ws/src/slam/package.xml b/ros2_ws/src/slam/package.xml deleted file mode 100644 index 987ba1e..0000000 --- a/ros2_ws/src/slam/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - slam - 0.0.0 - TODO: Package description - root - Apache-2.0 - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - ros2launch - - - ament_python - - diff --git a/ros2_ws/src/slam/resource/slam b/ros2_ws/src/slam/resource/slam deleted file mode 100644 index e69de29..0000000 diff --git a/ros2_ws/src/slam/setup.cfg b/ros2_ws/src/slam/setup.cfg deleted file mode 100644 index e16f645..0000000 --- a/ros2_ws/src/slam/setup.cfg +++ /dev/null @@ -1,6 +0,0 @@ -[develop] -script_dir=$base/lib/slam -[install] -install_scripts=$base/lib/slam -[build_scripts] -executable = /usr/bin/env python3 diff --git a/ros2_ws/src/slam/setup.py b/ros2_ws/src/slam/setup.py deleted file mode 100644 index 94c4fd1..0000000 --- a/ros2_ws/src/slam/setup.py +++ /dev/null @@ -1,31 +0,0 @@ -from setuptools import find_packages, setup -import os -from glob import glob - -package_name = "slam" - -setup( - name=package_name, - version="0.0.0", - packages=find_packages(exclude=["test"]), - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - (os.path.join("share", package_name, "launch"), glob("launch/*.py")), - (os.path.join("share", package_name, "config"), glob("config/*")), - ], - install_requires=["setuptools"], - zip_safe=True, - maintainer="root", - maintainer_email="root@todo.todo", - description="TODO: Package description", - license="Apache-2.0", - extras_require={ - "test": [ - "pytest", - ], - }, - entry_points={ - "console_scripts": ["scan_mavlink_bridge = slam.scan_mavlink_bridge:main"], - }, -) diff --git a/ros2_ws/src/slam/slam/__init__.py b/ros2_ws/src/slam/slam/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/ros2_ws/src/slam/slam/scan_mavlink_bridge.py b/ros2_ws/src/slam/slam/scan_mavlink_bridge.py deleted file mode 100755 index 41c0abd..0000000 --- a/ros2_ws/src/slam/slam/scan_mavlink_bridge.py +++ /dev/null @@ -1,80 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import Imu -from pymavlink import mavutil - - -class RawImuBridge(Node): - def __init__(self): - # Create Node - super().__init__("raw_imu_bridge") - - self.imu_pub = self.create_publisher(Imu, "/imu/data_raw", 50) - - # Connect the Pixhawk - self.port = "/dev/ttyACM0" - self.baud = 115200 - - try: - self.master = mavutil.mavlink_connection(self.port, baud=self.baud) - self.get_logger().info(f"Opened serial port {self.port}") - - # Wait for heartbeat - self.master.wait_heartbeat() - self.get_logger().info("Heartbeat received") - - self.master.mav.request_data_stream_send( - self.master.target_system, - self.master.target_component, - mavutil.mavlink.MAV_DATA_STREAM_RAW_SENSORS, - 50, # Hz - 1, # start streaming - ) - self.get_logger().info("Requested RAW_IMU stream (50 Hz)") - - self.timer = self.create_timer(0.02, self.read_serial) - - except Exception as e: - self.get_logger().error(f"Failed to connect: {e}") - rclpy.shutdown() - - # Obtain IMU data - def read_serial(self): - """Read RAW_IMU messages and publish to ROS topic.""" - msg = self.master.recv_match(type="RAW_IMU", blocking=False) - if not msg: - return - - imu_msg = Imu() - imu_msg.header.stamp = self.get_clock().now().to_msg() - imu_msg.header.frame_id = "imu_link" - - imu_msg.linear_acceleration.x = float(msg.xacc) - imu_msg.linear_acceleration.y = float(msg.yacc) - imu_msg.linear_acceleration.z = float(msg.zacc) - - imu_msg.angular_velocity.x = float(msg.xgyro) - imu_msg.angular_velocity.y = float(msg.ygyro) - imu_msg.angular_velocity.z = float(msg.zgyro) - - # Orientation unknown - imu_msg.orientation_covariance[0] = -1 - - # Publish IMU message - self.imu_pub.publish(imu_msg) - - self.get_logger().info("Published IMU from RAW_IMU") - - -def main(args=None): - rclpy.init(args=args) - node = RawImuBridge() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/ros2_ws/src/slam/test/test_copyright.py b/ros2_ws/src/slam/test/test_copyright.py deleted file mode 100644 index ceffe89..0000000 --- a/ros2_ws/src/slam/test/test_copyright.py +++ /dev/null @@ -1,27 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip( - reason="No copyright header has been placed in the generated source file." -) -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found errors" diff --git a/ros2_ws/src/slam/test/test_flake8.py b/ros2_ws/src/slam/test/test_flake8.py deleted file mode 100644 index ee79f31..0000000 --- a/ros2_ws/src/slam/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len( - errors - ) + "\n".join(errors) diff --git a/ros2_ws/src/slam/test/test_pep257.py b/ros2_ws/src/slam/test/test_pep257.py deleted file mode 100644 index a2c3deb..0000000 --- a/ros2_ws/src/slam/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found code style errors / warnings" diff --git a/viso_test/drone_connection.py b/viso_test/drone_connection.py new file mode 100644 index 0000000..fc8cd1d --- /dev/null +++ b/viso_test/drone_connection.py @@ -0,0 +1,141 @@ +from pymavlink import mavutil +import time + + +class DroneConnection: + __create_key = object() + + def __init__(self, drone): + self.drone = drone + self.time = time.monotonic() + + @classmethod + def create( + cls, address: str = "/dev/ttyAMA0", baud: int = 57600 + ) -> "tuple[bool, DroneConnection | None]": + drone = mavutil.mavlink_connection( + address, baud=baud, source_component=191, source_system=1 + ) + drone.wait_heartbeat() + print("Heartbeat from drone successfully received") + + return True, DroneConnection(drone) + + def send_odometry(self, x, y, z, q, vx, vy, vz, angular_vx, angular_vy, angular_vz): + """ + Sends an ODOMETRY MAVLink message. + + :param master: Pymavlink connection object. + :param time_usec: Timestamp (microseconds, since boot or Unix epoch). + :param x, y, z: Position in meters (e.g., NED frame). + :param q: Quaternion for orientation [w, x, y, z]. + :param vx, vy, vz: Linear velocity in m/s. + :param angular_vx, angular_vy, angular_vz: Angular velocity in rad/s. + """ + frame_id = mavutil.mavlink.MAV_FRAME_LOCAL_FRD + child_frame_id = ( + mavutil.mavlink.MAV_FRAME_BODY_FRD + ) # often used for velocity estimates + + time_usec = int((time.monotonic() - self.time) * 1e6) + + self.drone.mav.odometry_send( + time_usec, # time_usec (uint64_t) + frame_id, # frame_id (uint8_t) + child_frame_id, # child_frame_id (uint8_t) + x, + y, + z, # x, y, z (float, meters) + q, # q (float[4], quaternion w, x, y, z) + vx, + vy, + vz, # vx, vy, vz (float, m/s) + angular_vx, # angular_vx (float, rad/s) + angular_vy, # angular_vy (float, rad/s) + angular_vz, # angular_vz (float, rad/s) + # Start of pose covariance matrix + [0.01, 0.0, 0.0, 0.0, 0.0, 0.0] + + [0.01, 0.0, 0.0, 0.0, 0.0] + + [0.01, 0.0, 0.0, 0.0] + + [0.005, 0.0, 0.0] + + [0.005, 0.0] + + [0.01], + # End of pose covariance matrix + # Start of velocity covariance matrix + [0.04, 0.0, 0.0, 0.0, 0.0, 0.0] + + [0.04, 0.0, 0.0, 0.0, 0.0] + + [0.04, 0.0, 0.0, 0.0] + + [0.02, 0.0, 0.0] + + [0.02, 0.0] + + [0.05], + # End of velocity covariance matrix + 0, # reset_counter (uint8_t, 0 for no reset) + mavutil.mavlink.MAV_ESTIMATOR_TYPE_VIO, # estimator type + ) + + def send_vision_position_estimate(self, x, y, z, roll, pitch, yaw): + """ + Sends an VISION_POSITION_ESTIMATE MAVLink message. + + :param x, y, z: Position in meters + :param roll, pitch, yaw: angles in radians + """ + time_usec = int(time.time() * 1e6) + + self.drone.mav.vision_position_estimate_send( + usec=time_usec, + x=x, + y=y, + z=z, + roll=roll, + pitch=pitch, + yaw=yaw, + covariance=[0.0001, 0.0, 0.0, 0.0, 0.0, 0.0] + + [0.0001, 0.0, 0.0, 0.0, 0.0] + + [0.0001, 0.0, 0.0, 0.0] + + [0.00005, 0.0, 0.0] + + [0.00005, 0.0] + + [0.0001], + ) + + def send_vision_speed_estimate(self, x, y, z): + """ + Sends an VISION_SPEED_ESTIMATE MAVLink message + + :param x, y, z: speeds in meters/seconds + """ + time_usec = int(time.time() * 1e6) + + self.drone.mav.vision_speed_estimate_send( + usec=time_usec, + x=x, + y=y, + z=z, + covariance=[0.0004, 0.0, 0.0, 0.0, 0.0, 0.0] + + [0.0004, 0.0, 0.0, 0.0, 0.0] + + [0.0004, 0.0, 0.0, 0.0] + + [0.0002, 0.0, 0.0] + + [0.0002, 0.0] + + [0.0005], + ) + + def init_position(self): + """ + Function for positions that need to be sent at the start of the script + """ + self.drone.mav.set_gps_global_origin_send( + target_system=1, latitude=0, longitude=0, altitude=0 + ) + self.drone.mav.set_home_position_send( + target_system=1, + latitude=0, + longitude=0, + altitude=0, + x=0, + y=0, + z=0, + q=[1, 0, 0, 0], + approach_x=0, + approach_y=0, + approach_z=1, + ) diff --git a/viso_test/main.py b/viso_test/main.py new file mode 100644 index 0000000..d249c59 --- /dev/null +++ b/viso_test/main.py @@ -0,0 +1,154 @@ +import time +import depthai as dai +import numpy as np +import math + +USE_RERUN = False +USE_MAVLINK = True +USE_VISUAL_POSITION = True + +if USE_MAVLINK: + from drone_connection import DroneConnection + + success, drone_connection = DroneConnection.create() + if not success: + print("Failed to connect to drone") + exit(1) + +if USE_RERUN: + from rerun_node import RerunNode + + +# functions +def quat_to_euler(qw, qx, qy, qz): + roll = math.atan2(2 * (qw * qx + qy * qz), 1 - 2 * (qx * qx + qy * qy)) + pitch = math.asin(2 * (qw * qy - qz * qx)) + yaw = math.atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz)) + return -roll, pitch, -yaw + + +# Create pipeline + +try: + with dai.Pipeline() as p: + fps = 60 + width = 640 + height = 400 + # Define sources and outputs + left = p.create(dai.node.Camera).build( + dai.CameraBoardSocket.CAM_B, sensorFps=fps + ) + right = p.create(dai.node.Camera).build( + dai.CameraBoardSocket.CAM_C, sensorFps=fps + ) + imu = p.create(dai.node.IMU) + odom = p.create(dai.node.BasaltVIO) + slam = p.create(dai.node.RTABMapSLAM) + stereo = p.create(dai.node.StereoDepth) + params = { + "RGBD/CreateOccupancyGrid": "true", + "Grid/3D": "true", + "Rtabmap/SaveWMState": "true", + } + slam.setParams(params) + + if USE_RERUN: + rerunViewer = RerunNode() + imu.enableIMUSensor( + [dai.IMUSensor.ACCELEROMETER, dai.IMUSensor.GYROSCOPE_CALIBRATED], 200 + ) + imu.setBatchReportThreshold(1) + imu.setMaxBatchReports(10) + + stereo.setExtendedDisparity(False) + stereo.setLeftRightCheck(True) + stereo.setSubpixel(True) + stereo.setRectifyEdgeFillColor(0) + stereo.enableDistortionCorrection(True) + stereo.initialConfig.setLeftRightCheckThreshold(10) + stereo.setDepthAlign(dai.CameraBoardSocket.CAM_B) + + left.requestOutput((width, height)).link(stereo.left) + right.requestOutput((width, height)).link(stereo.right) + stereo.syncedLeft.link(odom.left) + stereo.syncedRight.link(odom.right) + stereo.depth.link(slam.depth) + stereo.rectifiedLeft.link(slam.rect) + imu.out.link(odom.imu) + + odom.transform.link(slam.odom) + + if USE_RERUN: + slam.transform.link(rerunViewer.inputTrans) + slam.passthroughRect.link(rerunViewer.inputImg) + slam.occupancyGridMap.link(rerunViewer.inputGrid) + slam.obstaclePCL.link(rerunViewer.inputObstaclePCL) + slam.groundPCL.link(rerunViewer.inputGroundPCL) + + slamQueue = slam.transform.createOutputQueue(maxSize=4, blocking=False) + odomQueue = odom.transform.createOutputQueue(maxSize=4, blocking=False) + imuQueue = imu.out.createOutputQueue(maxSize=4, blocking=False) + + p.start() + if USE_MAVLINK and drone_connection is not None: + drone_connection.init_position() + + while p.isRunning(): + odomData = odomQueue.tryGet() + imuData = imuQueue.tryGet() + if odomData is not None: + print("Odom:") + # print(slamData) + print("-----") + print(f"X -> Forward: {-odomData.getTranslation().x}") + print(f"Y -> Right: {odomData.getTranslation().y}") + print(f"Z -> Down: {-odomData.getTranslation().z}") + + if USE_MAVLINK: + x = -odomData.getTranslation().x + y = odomData.getTranslation().y + z = -odomData.getTranslation().z + qw = odomData.getQuaternion().qw + qx = odomData.getQuaternion().qx + qy = odomData.getQuaternion().qy + qz = odomData.getQuaternion().qz + + roll, pitch, yaw = quat_to_euler(qw, qx, qy, qz) + print("-----") + print(f"Roll -> Right: {roll}") + print(f"Pitch -> Up: {pitch}") + print(f"Yaw -> Right: {yaw}") + + if USE_VISUAL_POSITION: + roll, pitch, yaw = quat_to_euler(qw, qx, qy, qz) + drone_connection.send_vision_position_estimate( + x, y, z, roll, pitch, yaw + ) + else: + # TODO: Do these please + vx = vy = vz = 0.0 + + if imuData is not None: + gyro = imuData.packets[-1].gyroscope + angular_vx = gyro.z + angular_vy = gyro.x + angular_vz = gyro.y + else: + angular_vx = angular_vy = angular_vz = 0.0 + + drone_connection.send_odometry( + x, + y, + z, + [qw, qx, qy, qz], + vx, + vy, + vz, + angular_vx, + angular_vy, + angular_vz, + ) + time.sleep(0.067) +except Exception as e: + print(f"Error: {e}") + exit(1) diff --git a/viso_test/requirements.txt b/viso_test/requirements.txt new file mode 100644 index 0000000..4623599 --- /dev/null +++ b/viso_test/requirements.txt @@ -0,0 +1,5 @@ +depthai +numpy +opencv-python +pymavlink +rerun-sdk \ No newline at end of file diff --git a/viso_test/rerun_node.py b/viso_test/rerun_node.py new file mode 100644 index 0000000..db9e01b --- /dev/null +++ b/viso_test/rerun_node.py @@ -0,0 +1,98 @@ +import depthai as dai +import sys + +from pathlib import Path + +installExamplesStr = ( + Path(__file__).absolute().parents[2] / "install_requirements.py --install_rerun" +) +try: + import rerun as rr +except ImportError: + sys.exit( + "Critical dependency missing: Rerun. Please install it using the command: '{} {}' and then rerun the script.".format( + sys.executable, installExamplesStr + ) + ) + +import cv2 + + +class RerunNode(dai.node.ThreadedHostNode): + def __init__(self): + dai.node.ThreadedHostNode.__init__(self) + self.inputTrans = dai.Node.Input(self) + self.inputImg = dai.Node.Input(self) + self.inputObstaclePCL = dai.Node.Input(self) + self.inputGroundPCL = dai.Node.Input(self) + self.inputGrid = dai.Node.Input(self) + self.positions = [] + self.fx = 400.0 + self.fy = 400.0 + self.intrinsicsSet = False + + def getFocalLengthFromImage(self, imgFrame): + p = self.getParentPipeline() + calibHandler = p.getDefaultDevice().readCalibration() + intrinsics = calibHandler.getCameraIntrinsics( + dai.CameraBoardSocket(imgFrame.getInstanceNum()), + imgFrame.getWidth(), + imgFrame.getHeight(), + ) + self.fx = intrinsics[0][0] + self.fy = intrinsics[1][1] + self.intrinsicsSet = True + + def run(self): + rr.init("", spawn=True) + rr.log("world", rr.ViewCoordinates.FLU) + rr.log("world/ground", rr.Boxes3D(half_sizes=[3.0, 3.0, 0.00001])) + while self.isRunning(): + transData = self.inputTrans.get() + imgFrame = self.inputImg.get() + if not self.intrinsicsSet: + self.getFocalLengthFromImage(imgFrame) + pclObstData = self.inputObstaclePCL.tryGet() + pclGrndData = self.inputGroundPCL.tryGet() + mapData = self.inputGrid.tryGet() + if transData is not None: + trans = transData.getTranslation() + quat = transData.getQuaternion() + position = rr.datatypes.Vec3D([trans.x, trans.y, trans.z]) + rr.log( + "world/camera", + rr.Transform3D( + translation=position, + rotation=rr.datatypes.Quaternion( + xyzw=[quat.qx, quat.qy, quat.qz, quat.qw] + ), + ), + ) + self.positions.append(position) + lineStrip = rr.components.LineStrip3D(self.positions) + rr.log("world/trajectory", rr.LineStrips3D(lineStrip)) + rr.log( + "world/camera/image", + rr.Pinhole( + resolution=[imgFrame.getWidth(), imgFrame.getHeight()], + focal_length=[self.fx, self.fy], + camera_xyz=rr.ViewCoordinates.FLU, + ), + ) + img = imgFrame.getCvFrame() + img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) + rr.log("world/camera/image/rgb", rr.Image(img)) + if pclObstData is not None: + points, colors = pclObstData.getPointsRGB() + rr.log( + "world/obstacle_pcl", + rr.Points3D(points, colors=colors, radii=[0.01]), + ) + if pclGrndData is not None: + points, colors = pclGrndData.getPointsRGB() + rr.log( + "world/ground_pcl", + rr.Points3D(points, colors=colors, radii=[0.01]), + ) + if mapData is not None: + rr.log("map", rr.Image(mapData.getCvFrame()))