├── .clang-format ├── .clang-tidy ├── .cmake-format.py ├── .dvc ├── .gitignore └── config ├── .dvcignore ├── .github └── workflows │ └── test.yml ├── .gitignore ├── .pre-commit-config.yaml ├── .vscode ├── c_cpp_properties.json └── settings.json ├── .yamllint-config.yaml ├── Dockerfile ├── Makefile ├── README.md ├── doc ├── Makefile ├── README.md ├── ackermann_vehicle.md ├── foxglove.md ├── gearbox.md ├── getting_started.md ├── images │ └── gearbox.png ├── nodes_and_topics.md ├── svg │ └── ackermann_vehicle.svg └── tex │ └── ackermann_vehicle.tex ├── docker-compose.yaml ├── layout └── pure_pursuit.json ├── packages ├── README.md ├── collision │ ├── CMakeLists.txt │ ├── REAME.md │ ├── include │ │ └── collision │ │ │ ├── collision_checker.h │ │ │ └── map.h │ ├── package.xml │ └── src │ │ ├── collision_checker.cpp │ │ └── map.cpp ├── common │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ │ └── common │ │ │ ├── array_as_bit.h │ │ │ ├── array_as_queue.h │ │ │ ├── exception.h │ │ │ ├── format.h │ │ │ ├── lock.h │ │ │ ├── math.h │ │ │ └── result.h │ ├── package.xml │ ├── src │ │ └── format.cpp │ └── tests │ │ ├── CMakeLists.txt │ │ └── main.cpp ├── control_proxy │ ├── CMakeLists.txt │ ├── README.md │ ├── launch │ │ └── control_proxy.yaml │ ├── package.xml │ └── src │ │ ├── control_proxy_node.cpp │ │ ├── control_proxy_node.h │ │ └── main.cpp ├── fastgrid │ ├── CMakeLists.txt │ ├── benchmark │ │ ├── CMakeLists.txt │ │ ├── main.cpp │ │ ├── test_large.txt │ │ ├── test_medium.txt │ │ └── test_small.txt │ ├── include │ │ └── fastgrid │ │ │ ├── distance_transform.h │ │ │ ├── draw.h │ │ │ ├── grid.h │ │ │ ├── holder.h │ │ │ ├── interpolation.h │ │ │ ├── manhattan_distance.h │ │ │ └── misc.h │ ├── package.xml │ ├── src │ │ ├── distance_transform.cpp │ │ ├── draw.cpp │ │ ├── grid.cpp │ │ └── manhattan_distance.cpp │ └── test │ │ ├── CMakeLists.txt │ │ └── main.cpp ├── geom │ ├── CMakeLists.txt │ ├── include │ │ └── geom │ │ │ ├── angle.h │ │ │ ├── angle_vector.h │ │ │ ├── arc.h │ │ │ ├── bezier.h │ │ │ ├── boost.h │ │ │ ├── boost │ │ │ ├── box.h │ │ │ ├── linestring.h │ │ │ ├── point.h │ │ │ ├── polygon.h │ │ │ ├── ring.h │ │ │ └── segment.h │ │ │ ├── bounding_box.h │ │ │ ├── circle.h │ │ │ ├── common.h │ │ │ ├── complex_polygon.h │ │ │ ├── distance.h │ │ │ ├── intersection.h │ │ │ ├── line.h │ │ │ ├── localization.h │ │ │ ├── motion_state.h │ │ │ ├── msg.h │ │ │ ├── polygon.h │ │ │ ├── polyline.h │ │ │ ├── polyline_index.h │ │ │ ├── pose.h │ │ │ ├── ray.h │ │ │ ├── segment.h │ │ │ ├── swap.h │ │ │ ├── test │ │ │ └── equal_assert.h │ │ │ ├── transform.h │ │ │ ├── triangle.h │ │ │ ├── uniform_stepper.h │ │ │ ├── vector.h │ │ │ └── vector3.h │ ├── package.xml │ ├── src │ │ ├── angle.cpp │ │ ├── angle_vector.cpp │ │ ├── arc.cpp │ │ ├── bezier.cpp │ │ ├── bounding_box.cpp │ │ ├── circle.cpp │ │ ├── complex_polygon.cpp │ │ ├── distance.cpp │ │ ├── intersection.cpp │ │ ├── line.cpp │ │ ├── motion_state.cpp │ │ ├── msg.cpp │ │ ├── polygon.cpp │ │ ├── polyline.cpp │ │ ├── pose.cpp │ │ ├── segment.cpp │ │ ├── transform.cpp │ │ ├── vector.cpp │ │ └── vector3.cpp │ └── test │ │ ├── CMakeLists.txt │ │ ├── angle_tests.cpp │ │ ├── angle_vector_tests.cpp │ │ ├── arc_tests.cpp │ │ ├── boost_tests.cpp │ │ ├── bounding_box_tests.cpp │ │ ├── distance_tests.cpp │ │ ├── line_tests.cpp │ │ ├── main.cpp │ │ ├── polygon_tests.cpp │ │ ├── polyline_index_tests.cpp │ │ ├── ray_tests.cpp │ │ ├── segment_tests.cpp │ │ ├── uniform_stepper_tests.cpp │ │ ├── vector3_tests.cpp │ │ └── vector_tests.cpp ├── hardware_node │ ├── hardware_node │ │ ├── __init__.py │ │ ├── node.py │ │ └── teensy.py │ ├── launch │ │ └── hardware_node.yaml │ ├── package.xml │ ├── readme.md │ ├── resource │ │ ├── hardware_node │ │ └── steering.csv │ ├── setup.cfg │ └── setup.py ├── icp_odometry │ ├── CMakeLists.txt │ ├── config │ │ └── icp.yaml │ ├── include │ │ └── icp_odometry │ │ │ ├── common.h │ │ │ ├── conversion.h │ │ │ └── icp_odometry_node.h │ ├── launch │ │ └── icp_odometry.yaml │ ├── package.xml │ └── src │ │ ├── conversion.cpp │ │ ├── icp_odometry_node.cpp │ │ └── main.cpp ├── lidar_map │ ├── CMakeLists.txt │ ├── README.md │ ├── config │ │ └── icp.yaml │ ├── include │ │ └── lidar_map │ │ │ ├── builder.h │ │ │ ├── common.h │ │ │ ├── conversion.h │ │ │ └── serialization.h │ ├── notebook │ │ └── pose_graph_visualization.ipynb │ ├── package.xml │ └── src │ │ ├── builder.cpp │ │ ├── conversion.cpp │ │ ├── main.cpp │ │ └── serialization.cpp ├── map │ ├── CMakeLists.txt │ ├── data │ │ ├── map_1.geojson │ │ ├── map_2.geojson │ │ ├── map_3.geojson │ │ ├── map_4.geojson │ │ ├── map_5.geojson │ │ ├── map_6.geojson │ │ └── map_7.geojson │ ├── include │ │ └── map │ │ │ └── map.h │ ├── package.xml │ └── src │ │ └── map.cpp ├── model │ ├── CMakeLists.txt │ ├── README.md │ ├── config │ │ └── model.yaml │ ├── include │ │ └── model │ │ │ ├── model.h │ │ │ ├── params.h │ │ │ └── shape.h │ ├── launch │ │ └── model_tf.yaml │ ├── package.xml │ ├── src │ │ ├── model.cpp │ │ ├── model_tf.cpp │ │ ├── params.cpp │ │ ├── pybind.cpp │ │ └── shape.cpp │ └── test │ │ └── main.cpp ├── motion │ ├── CMakeLists.txt │ ├── include │ │ └── motion │ │ │ ├── primitive.h │ │ │ └── trajectory.h │ ├── package.xml │ └── src │ │ ├── primitive.cpp │ │ └── trajectory.cpp ├── navigation │ ├── CMakeLists.txt │ ├── include │ │ └── navigation │ │ │ ├── graph_builder.h │ │ │ ├── mesh_builder.h │ │ │ ├── search.h │ │ │ └── viewer.h │ ├── package.xml │ ├── src │ │ ├── graph_builder.cpp │ │ ├── mesh_builder.cpp │ │ ├── search.cpp │ │ └── viewer.cpp │ └── test │ │ └── main.cpp ├── occupancy_grid │ ├── CMakeLists.txt │ ├── include │ │ └── occupancy_grid │ │ │ └── occupancy_grid_node.h │ ├── launch │ │ └── occupancy_grid.yaml │ ├── package.xml │ └── src │ │ ├── main.cpp │ │ └── occupancy_grid_node.cpp ├── perf │ ├── CMakeLists.txt │ ├── include │ │ └── perf │ │ │ ├── node.h │ │ │ └── stat.h │ ├── launch │ │ └── perf_stat.yaml │ ├── package.xml │ └── src │ │ ├── main.cpp │ │ ├── node.cpp │ │ └── stat.cpp ├── planner │ ├── CMakeLists.txt │ ├── data │ │ └── primitives.ipynb │ ├── include │ │ └── planner │ │ │ ├── planner_node.h │ │ │ └── search.h │ ├── launch │ │ └── planner.yaml │ ├── package.xml │ ├── primitives.json │ └── src │ │ ├── main.cpp │ │ ├── planner_node.cpp │ │ └── search.cpp ├── pure_pursuit │ ├── CMakeLists.txt │ ├── include │ │ └── pure_pursuit │ │ │ ├── pure_pursuit.h │ │ │ ├── pure_pursuit_node.h │ │ │ └── simulator.hpp │ ├── launch │ │ └── pure_pursuit.yaml │ ├── package.xml │ └── src │ │ ├── main.cpp │ │ ├── pure_pursuit.cpp │ │ └── pure_pursuit_node.cpp ├── route_follower │ ├── CMakeLists.txt │ ├── include │ │ └── route_follower │ │ │ └── route_follower_node.h │ ├── launch │ │ └── route_follower.yaml │ ├── package.xml │ └── src │ │ ├── main.cpp │ │ └── route_follower_node.cpp ├── routing │ ├── CMakeLists.txt │ ├── include │ │ └── routing │ │ │ └── routing_node.h │ ├── launch │ │ └── routing.yaml │ ├── package.xml │ └── src │ │ ├── main.cpp │ │ └── routing_node.cpp ├── simulator_2d │ ├── CMakeLists.txt │ ├── README.md │ ├── doc │ │ └── svg │ │ │ ├── imu.svg │ │ │ └── lidar.svg │ ├── include │ │ └── simulator_2d │ │ │ ├── simulation_map.h │ │ │ ├── simulator_engine.h │ │ │ ├── status_code.h │ │ │ └── truck_state.h │ ├── launch │ │ └── simulator_2d.yaml │ ├── package.xml │ ├── src │ │ ├── main.cpp │ │ ├── simulation_map.cpp │ │ ├── simulator_engine.cpp │ │ ├── simulator_node.cpp │ │ ├── simulator_node.h │ │ └── truck_state.cpp │ └── test │ │ ├── main.cpp │ │ └── simulation_map_tests.cpp ├── speed │ ├── CMakeLists.txt │ ├── include │ │ └── speed │ │ │ └── greedy_planner.h │ ├── package.xml │ └── src │ │ └── greedy_planner.cpp ├── svg_debug_drawer │ ├── CMakeLists.txt │ ├── include │ │ └── svg_debug_drawer │ │ │ └── sdd.h │ ├── package.xml │ ├── src │ │ └── sdd.cpp │ └── test │ │ ├── data │ │ └── im1_in.svg │ │ └── main.cpp ├── truck │ ├── CMakeLists.txt │ ├── README.md │ ├── config │ │ └── point_matcher.yaml │ ├── launch │ │ ├── bridge.yaml │ │ ├── camera.yaml │ │ ├── common.yaml │ │ ├── control.yaml │ │ ├── demo.yaml │ │ ├── graph.yaml │ │ ├── joy.yaml │ │ ├── lidar.yaml │ │ ├── remote.yaml │ │ ├── route_follower_demo.yaml │ │ ├── routing_demo.yaml │ │ ├── simulator.yaml │ │ ├── slam.yaml │ │ └── truck.yaml │ └── package.xml ├── truck_msgs │ ├── CMakeLists.txt │ ├── msg │ │ ├── Control.msg │ │ ├── ControlMode.msg │ │ ├── CpuStat.msg │ │ ├── HardwareStatus.msg │ │ ├── HardwareTelemetry.msg │ │ ├── IcpOdometryStat.msg │ │ ├── MemStat.msg │ │ ├── MetaData.msg │ │ ├── NavigationMesh.msg │ │ ├── NavigationRoute.msg │ │ ├── PerfStat.msg │ │ ├── Point.msg │ │ ├── PurePursuitStatus.msg │ │ ├── RandomSeed.msg │ │ ├── RemoteControl.msg │ │ ├── SimulationState.msg │ │ ├── Trajectory.msg │ │ ├── TrajectoryState.msg │ │ └── Waypoints.msg │ └── package.xml ├── video_streaming │ ├── CMakeLists.txt │ ├── launch │ │ └── video_streaming.yaml │ ├── mediamtx.yaml │ └── package.xml ├── visualization │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ │ └── visualization │ │ │ ├── color.h │ │ │ ├── msg.h │ │ │ └── visualization_node.h │ ├── launch │ │ └── visualization.yaml │ ├── package.xml │ └── src │ │ ├── color.cpp │ │ ├── main.cpp │ │ ├── msg.cpp │ │ └── visualization_node.cpp └── waypoint_follower │ ├── CMakeLists.txt │ ├── include │ └── waypoint_follower │ │ ├── waypoint_follower.h │ │ └── waypoint_follower_node.h │ ├── launch │ └── waypoint_follower.yaml │ ├── package.xml │ └── src │ ├── main.cpp │ ├── waypoint_follower.cpp │ └── waypoint_follower_node.cpp ├── primitives ├── Generate_primitives.ipynb ├── README.md ├── out.json └── primitives.json ├── pyproject.toml ├── requirements.txt ├── scripts ├── pub.sh ├── rosenv.sh └── vehicle_params │ └── __main__.py └── setup ├── README.md ├── jetson.sh └── misc └── apt_preferences /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | Language: Cpp 3 | AlignTrailingComments: true 4 | AccessModifierOffset: -2 5 | AlignAfterOpenBracket: AlwaysBreak 6 | AllowShortFunctionsOnASingleLine: All 7 | BinPackArguments: false 8 | BreakBeforeBraces: Attach 9 | BreakBeforeBinaryOperators: NonAssignment 10 | BreakConstructorInitializers: AfterColon 11 | ColumnLimit: 100 12 | CompactNamespaces: false 13 | IndentWidth: 4 14 | NamespaceIndentation: None 15 | DerivePointerAlignment: false 16 | PointerAlignment: Left 17 | QualifierAlignment: Left 18 | SortIncludes: false 19 | SpaceAfterTemplateKeyword: false 20 | SpaceInEmptyParentheses: false 21 | SpaceBeforeAssignmentOperators: true 22 | SpaceBeforeParens: ControlStatements 23 | -------------------------------------------------------------------------------- /.dvc/.gitignore: -------------------------------------------------------------------------------- 1 | /config.local 2 | /tmp 3 | /cache 4 | -------------------------------------------------------------------------------- /.dvc/config: -------------------------------------------------------------------------------- 1 | [core] 2 | remote = public 3 | ['remote "public"'] 4 | url = https://the-lab-bucket.storage.yandexcloud.net/dvc/ 5 | -------------------------------------------------------------------------------- /.dvcignore: -------------------------------------------------------------------------------- 1 | # Add patterns of files dvc should ignore, which could improve 2 | # the performance. Learn more at 3 | # https://dvc.org/doc/user-guide/dvcignore 4 | -------------------------------------------------------------------------------- /.github/workflows/test.yml: -------------------------------------------------------------------------------- 1 | name: Test 2 | 3 | on: 4 | pull_request: 5 | branches: [master] 6 | push: 7 | branches: [master] 8 | 9 | concurrency: 10 | group: ${{ github.workflow }}-${{ github.ref }} 11 | cancel-in-progress: true 12 | 13 | jobs: 14 | pre-commit: 15 | name: Run pre-commit hooks 16 | runs-on: ubuntu-latest 17 | steps: 18 | - uses: actions/checkout@v4 19 | - uses: actions/setup-python@v5 20 | with: { python-version: "3.10" } 21 | - uses: pre-commit/action@v3.0.1 22 | 23 | extract-image: 24 | name: Get image version 25 | runs-on: ubuntu-latest 26 | outputs: 27 | image-url: ${{ steps.extractor.outputs.image_url }} 28 | steps: 29 | - uses: actions/checkout@v4 30 | - uses: actions/setup-python@v5 31 | with: { python-version: "3.10" } 32 | 33 | - name: Install dependencies 34 | run: | 35 | sudo pip install yq 36 | sudo apt-get install -y jq 37 | 38 | - name: Extract image URL 39 | id: extractor 40 | run: | 41 | IMAGE_URL=$(cat docker-compose.yaml | yq ".services.${{ github.event.repository.name }}.image") 42 | echo "image_url=$IMAGE_URL" | tee -a $GITHUB_OUTPUT 43 | 44 | build-and-check: 45 | name: Build, test 46 | needs: extract-image 47 | runs-on: self-hosted 48 | container: 49 | image: ${{ needs.extract-image.outputs.image-url }} 50 | defaults: 51 | run: { shell: bash } 52 | 53 | steps: 54 | - uses: actions/checkout@v4 55 | 56 | - name: Build ROS2 packages 57 | run: make build-all 58 | 59 | - name: Test ROS2 packages 60 | run: make test-all 61 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.DS_Store 2 | .vscode 3 | .dvc 4 | __pycache__/ 5 | *.py[cod] 6 | *$py.class 7 | 8 | # ROS2 builds 9 | build 10 | install 11 | log 12 | 13 | # Test data 14 | packages/navigation/test/data 15 | packages/simulator_2d/test/data 16 | packages/svg_debug_drawer/test/data/im*_out.svg 17 | packages/lidar_map/data 18 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | # Common sanity checks 3 | - repo: https://github.com/pre-commit/pre-commit-hooks 4 | rev: v4.5.0 5 | hooks: 6 | - id: check-added-large-files 7 | args: [--enforce-all, --maxkb=12300] 8 | - id: check-merge-conflict 9 | - id: check-xml 10 | - id: check-json 11 | - id: check-yaml 12 | - id: check-toml 13 | - id: end-of-file-fixer 14 | - id: mixed-line-ending 15 | - id: trailing-whitespace 16 | 17 | # YAML linter 18 | - repo: https://github.com/adrienverge/yamllint 19 | rev: v1.35.1 20 | hooks: 21 | - id: yamllint 22 | exclude: ^.clang-tidy$ 23 | args: [--strict, -d, ".yamllint-config.yaml"] 24 | 25 | # CMake linter and formatter 26 | - repo: https://github.com/cheshirekow/cmake-format-precommit 27 | rev: v0.6.10 28 | hooks: 29 | - id: cmake-format 30 | - id: cmake-lint 31 | 32 | # Python linters & formatters 33 | - repo: https://github.com/astral-sh/ruff-pre-commit 34 | rev: v0.1.3 35 | hooks: 36 | - id: ruff 37 | args: [--fix, --show-fixes] 38 | - id: ruff-format 39 | 40 | # C++ formatter 41 | - repo: https://github.com/pre-commit/mirrors-clang-format 42 | rev: v17.0.6 43 | hooks: 44 | - id: clang-format 45 | types_or: [c, c++] 46 | -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Linux", 5 | "includePath": [ 6 | "${workspaceFolder}/**", 7 | "${ROS_ROOT}/**", 8 | "${workspaceFolder}/packages/build/**", 9 | "/usr/local/**", 10 | "/usr/include/**" 11 | ], 12 | "defines": [], 13 | "compilerPath": "/usr/bin/gcc", 14 | "cStandard": "gnu17", 15 | "cppStandard": "gnu++20", 16 | "intelliSenseMode": "linux-gcc-x64", 17 | "configurationProvider": "ms-vscode.cmake-tools" 18 | } 19 | ], 20 | "version": 4 21 | } 22 | -------------------------------------------------------------------------------- /.yamllint-config.yaml: -------------------------------------------------------------------------------- 1 | rules: 2 | line-length: 3 | max: 120 4 | 5 | quoted-strings: 6 | quote-type: double 7 | required: false 8 | 9 | braces: 10 | min-spaces-inside: 1 11 | max-spaces-inside: 1 12 | min-spaces-inside-empty: 0 13 | max-spaces-inside-empty: 0 14 | 15 | brackets: 16 | min-spaces-inside-empty: 0 17 | max-spaces-inside-empty: 0 18 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![Test](https://github.com/robotics-laboratory/truck/actions/workflows/test.yml/badge.svg?branch=master)](https://github.com/robotics-laboratory/truck/actions/workflows/test.yml) 2 | 3 | # Truck 4 | 5 | **Truck** is an open source project dedicated to the construction and development of the 2.5D indoor autonomous vehicle based on the [ackermann steering model](https://github.com/robotics-laboratory/truck/blob/master/doc/ackermann_vehicle.md). It's maintained by the Robotics Group (Faculty of Computer Science, HSE University) and enthusiasts. You may follow our progress on [YouTube](https://www.youtube.com/watch?v=hF6cDalz8-I&list=PLR1nN_AQOO9zHpkW-phZnqVywjUCj7zHZ). 6 | 7 | It is intended to be an educational reasearch project, so, among our goals is to maintain a rich set of study materials and detailed [documentation](https://github.com/robotics-laboratory/truck/blob/master/doc/README.md). 8 | -------------------------------------------------------------------------------- /doc/Makefile: -------------------------------------------------------------------------------- 1 | .PHONY: all 2 | 3 | all: ackermann_vehicle_svg 4 | 5 | build_dir: 6 | mkdir -p build 7 | 8 | ackermann_vehicle_svg: build_dir 9 | latex -output-directory build tex/ackermann_vehicle.tex 10 | dvisvgm --no-fonts build/ackermann_vehicle.dvi -o svg/* 11 | -------------------------------------------------------------------------------- /doc/README.md: -------------------------------------------------------------------------------- 1 | # Docs 2 | 3 | ### Overview 4 | - [Ackermann model](ackermann_vehicle.md) 5 | - [Nodes & topics](nodes_and_topics.md) 6 | 7 | ### Dev 8 | - [Getting started](getting_started.md) 9 | - [Foxglove](foxglove.md) 10 | - [CPP guides](https://github.com/robotics-laboratory/cookbook/blob/master/cpp/style.md) 11 | 12 | ### Build docs 13 | To rebuild docs run `make` in current dir. 14 | -------------------------------------------------------------------------------- /doc/ackermann_vehicle.md: -------------------------------------------------------------------------------- 1 | # Ackermann vehicle 2 | 3 | Ackermann steering geometry is a geometric arrangement of linkages in the steering of vehicle designed to solve the problem of wheels on the inside and outside of a turn needing to trace out circles of different radius. The intention of Ackermann geometry is to avoid the need for tires to slip sideways when following the path around a curve. The geometrical solution to this is for all wheels to have their axles arranged as radius of circles with a common centre point. As the rear wheels are fixed, this centre point must be on a line extended from the rear axle. Intersecting the axes of the front wheels on this line as well requires that the inside front wheel be turned, when steering, through a greater angle than the outside wheel. 4 | 5 | In our implementation, each front wheel is controlled by a separate servo. 6 | 7 | ## Model 8 | State of system is $(x, y, \theta, v, C)$: 9 | - $(x, y)$ – position of base point in the world 10 | - $\theta$ - yaw, current orientation in the world 11 | - $v$ – instant linear velocity of base point 12 | - $C$ – instant curvature ob base point movement 13 | 14 | ![This is an image](svg/ackermann_vehicle.svg) 15 | 16 | Control input is tuple of curvature, desired linear speed and acceleration $(C, v, a)$, where $C > 0$ is left. 17 | We make the assumption that steering angle of front wheels is achieved instantly, but consider desired acceleration to achieve target velocity for more smooth control. 18 | 19 | System has the following limits: 20 | - Velocity limited $v \in [v_{min}, v_{max}]$, backward movement is possible ($v_{min} < 0$). 21 | - Acceleration is limited $a \in [a_{min}, a_{max}]$. 22 | - The center of rotation is out of chassis $|С| < \big( l_b^2 + (\frac{w}{2} + \epsilon)^2 \big)^{-\frac{1}{2}}$. 23 | - Wheels steering is constrained with the link system $\delta_l, \delta_r \in [\delta_{min}, \delta_{max}]$, where ($\delta > 0$) is left. 24 | -------------------------------------------------------------------------------- /doc/foxglove.md: -------------------------------------------------------------------------------- 1 | # Foxglove 2 | 3 | ## Getting started 4 | We use [foxglove](https://foxglove.dev) as visualization tool for our project. Download [Foxglove studio](https://foxglove.dev/download) to your machine or jetson. And look at official [docs](https://foxglove.dev/docs/studio). 5 | 6 | At current moment we use [rosbridge](http://wiki.ros.org/rosbridge_suite) to communicate with ROS2, so use this connection method at live robot. 7 | - For remote access use ```ws://jetson.local:9090``` (recomended to connect jetson as wifi hotspot). 8 | - For local run just use address ```ws://localhost:9090```. 9 | 10 | ## Layouts 11 | We have several launch configs in package 'truck'. Some layout is prepared for each of them. Freely use it, exporting from repo to foxglove studio. 12 | -------------------------------------------------------------------------------- /doc/gearbox.md: -------------------------------------------------------------------------------- 1 | # Gearbox & differential transmission 2 | 3 | - Motor to differential ratio: **(16 / 65) * (13 / 38) = 8 / 95 ≈ 0.08421** 4 | - Wheel diameter: **0.105 m** (radius: **0.0525 m**) 5 | - For each **100 turns** of motor rear wheels make **100 * 0.08421 ≈ 8.4 turns**, traveling distance of **8.4 * PI * 0.105 ≈ 2.77 m** 6 | 7 | [Edit in excalidraw](https://excalidraw.com/#json=YWuY4R6Dwvgntv3lw4HMn,Wu6pbtCz8jfEcrqzUD1GEQ) 8 | 9 | ![gearbox.png](images/gearbox.png) 10 | -------------------------------------------------------------------------------- /doc/images/gearbox.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-laboratory/truck/332ff42e67d083e36dcf9d8070f3c9e8527ffc6e/doc/images/gearbox.png -------------------------------------------------------------------------------- /doc/nodes_and_topics.md: -------------------------------------------------------------------------------- 1 | # Nodes & Topics 2 | 3 | ## Topics 4 | 5 | ### Planner 6 | - `/motion/trajectory` [[nav_msgs/Path]([nav_msgs/Path](http://docs.ros.org/en/lunar/api/nav_msgs/html/msg/Path.html))] - target motion trajectory 7 | - `/motion/command` [[truck_interfecase/Control](https://github.com/robotics-laboratory/truck/blob/master/packages/truck_interfaces/msg/Control.msg)] - target of trajectory follower 8 | 9 | ### Remote control 10 | - `/joy/set_feedback` [[sensos_msgs/JoyFeedback](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/JoyFeedback.html)] - joypad input topic (rumble, leds and etc) 11 | - `/joy` [[sensos_msgs/Joy](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Joy.html)] - joypad output topic 12 | 13 | ### Control 14 | - `/control/command` [[truck_interfecase/Control](https://github.com/robotics-laboratory/truck/blob/master/packages/truck_interfaces/msg/Control.msg)] - control target 15 | - `/control/mode` [[truck_interfecase/ControlMode](https://github.com/robotics-laboratory/truck/blob/master/packages/truck_interfaces/msg/ControlMode.msg)] - control proxy mode (auto or remote control) 16 | 17 | ## Nodes 18 | - [Control proxy](../packages/control_proxy/readme.md) 19 | -------------------------------------------------------------------------------- /docker-compose.yaml: -------------------------------------------------------------------------------- 1 | services: 2 | truck: 3 | container_name: "${CONTAINER_NAME:-truck-${USER}}" 4 | image: "registry.robotics-lab.ru/truck:1.0.0" 5 | privileged: true 6 | runtime: "${DOCKER_RUNTIME:-runc}" 7 | build: 8 | dockerfile: Dockerfile 9 | context: . 10 | x-bake: 11 | platforms: 12 | - linux/arm64 13 | - linux/amd64 14 | cache-from: 15 | - type=registry,ref=registry.robotics-lab.ru/truck:cache-arm64 16 | - type=registry,ref=registry.robotics-lab.ru/truck:cache-amd64 17 | cache-to: 18 | # - type=registry,ref=registry.robotics-lab.ru/truck:cache-arm64 19 | # - type=registry,ref=registry.robotics-lab.ru/truck:cache-amd64 20 | ports: 21 | - "${FOXGLOVE_PORT:-8765}:8765" 22 | - "${VIDEO_STREAMING_PORT:-8889}:8889" 23 | volumes: 24 | - "${PWD}:/truck" 25 | - "/dev:/dev" 26 | 27 | networks: 28 | default: 29 | name: "${CONTAINER_NAME:-truck-${USER}}" 30 | -------------------------------------------------------------------------------- /packages/README.md: -------------------------------------------------------------------------------- 1 | # ROS2 Packages 2 | 3 | - [control_proxy](control_proxy/README.md) 4 | - [model](model/README.md) 5 | - [truck](truck/README.md) 6 | 7 | ## Build 8 | 9 | To build all packages 10 | 11 | ```bash 12 | colcon build --merge-install 13 | ``` 14 | 15 | To build you some single package 16 | 17 | ```bash 18 | colcon build --merge-install --packages-up-to package_name 19 | ``` 20 | 21 | ## Install 22 | Current installation dir is ```/truck/packages/install```. To run setup do 23 | 24 | ```bash 25 | . install/setup.bash 26 | ``` 27 | -------------------------------------------------------------------------------- /packages/collision/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(collision) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | find_package(common REQUIRED) 7 | find_package(geom REQUIRED) 8 | find_package(model REQUIRED) 9 | find_package(nav_msgs REQUIRED) 10 | find_package(OpenCV REQUIRED) 11 | 12 | add_library(${PROJECT_NAME} SHARED src/collision_checker.cpp src/map.cpp) 13 | 14 | ament_target_dependencies(collision common geom model nav_msgs OpenCV) 15 | 16 | target_include_directories( 17 | collision PUBLIC "$" 18 | "$" 19 | ) 20 | 21 | ament_export_targets(collision HAS_LIBRARY_TARGET) 22 | 23 | ament_export_dependencies(common geom model nav_msgs OpenCV) 24 | 25 | install( 26 | TARGETS collision 27 | EXPORT collision 28 | ARCHIVE DESTINATION lib 29 | LIBRARY DESTINATION lib 30 | RUNTIME DESTINATION bin 31 | ) 32 | 33 | install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) 34 | 35 | ament_package() 36 | -------------------------------------------------------------------------------- /packages/collision/REAME.md: -------------------------------------------------------------------------------- 1 | # CollisonChecker 2 | Tiny library to evaluate distance to nearest obstacle in a 2D space (occupancy grid representation): 3 | - pre-compute distance to nearest obstacle for each cell in the map O(n) complexity 4 | - request for truck pose O(1) complexity 5 | 6 | ## Details 7 | Truck shape is decomposed as a set of intersecting circles. The distance to nearest obstacle is computed as a minimum distance to nearest circle. 8 | -------------------------------------------------------------------------------- /packages/collision/include/collision/collision_checker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "collision/map.h" 4 | 5 | #include "geom/pose.h" 6 | #include "geom/transform.h" 7 | #include "geom/vector.h" 8 | #include "model/params.h" 9 | 10 | #include 11 | 12 | #include 13 | 14 | namespace truck::collision { 15 | 16 | class StaticCollisionChecker { 17 | public: 18 | constexpr static double kMaxDistance = 10.0; 19 | 20 | StaticCollisionChecker(const model::Shape& shape); 21 | 22 | bool initialized() const; 23 | void reset(Map distance_transform); 24 | 25 | double distance(const geom::Pose& ego_pose) const; 26 | double distance(const geom::Vec2& point) const; 27 | 28 | private: 29 | model::Shape shape_; 30 | 31 | struct State { 32 | Map distance_transform; 33 | geom::Transform tf; 34 | }; 35 | 36 | std::optional state_ = std::nullopt; 37 | }; 38 | 39 | } // namespace truck::collision 40 | -------------------------------------------------------------------------------- /packages/collision/include/collision/map.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/pose.h" 4 | 5 | #include 6 | 7 | #include 8 | 9 | namespace truck::collision { 10 | 11 | struct Map { 12 | static Map fromOccupancyGrid(const nav_msgs::msg::OccupancyGrid& grid); 13 | Map emptyLikeThis() const; 14 | 15 | nav_msgs::msg::OccupancyGrid makeCostMap( 16 | const std_msgs::msg::Header& header, double kMaxDist) const; 17 | 18 | geom::Pose origin; 19 | double resolution; 20 | cv::Size size; 21 | cv::Mat data; 22 | }; 23 | 24 | Map distanceTransform(const Map& map); 25 | 26 | } // namespace truck::collision 27 | -------------------------------------------------------------------------------- /packages/collision/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | collision 5 | 0.0.0 6 | collision 7 | Milko Andrew 8 | MIT 9 | 10 | ament_cmake 11 | common 12 | geom 13 | model 14 | nav_msgs 15 | OpenCV 16 | 17 | 18 | ament_cmake 19 | 20 | 21 | -------------------------------------------------------------------------------- /packages/collision/src/collision_checker.cpp: -------------------------------------------------------------------------------- 1 | #include "collision/collision_checker.h" 2 | 3 | #include "common/exception.h" 4 | 5 | #include 6 | 7 | namespace truck::collision { 8 | 9 | StaticCollisionChecker::StaticCollisionChecker(const model::Shape& shape) : shape_(shape) {} 10 | 11 | bool StaticCollisionChecker::initialized() const { return state_.has_value(); } 12 | 13 | void StaticCollisionChecker::reset(Map distance_transform) { 14 | const auto& origin = distance_transform.origin; 15 | const auto tf = geom::Transform(origin.pos, origin.dir); 16 | 17 | state_ = State{ 18 | .distance_transform = std::move(distance_transform), 19 | .tf = tf.inv(), 20 | }; 21 | } 22 | 23 | double StaticCollisionChecker::distance(const geom::Vec2& point) const { 24 | VERIFY(initialized()); 25 | const auto grid_point = state_->tf(point); 26 | 27 | // find relevant indices of distance transform matrix 28 | const auto x = floor(grid_point.x / state_->distance_transform.resolution); 29 | const auto y = floor(grid_point.y / state_->distance_transform.resolution); 30 | 31 | // check borders 32 | if ((x < 0) || (y < 0) || (state_->distance_transform.size.height <= y) 33 | || (state_->distance_transform.size.width <= x)) { 34 | return kMaxDistance; 35 | } 36 | 37 | return state_->distance_transform.data.at(y, x) * state_->distance_transform.resolution; 38 | } 39 | 40 | double StaticCollisionChecker::distance(const geom::Pose& ego_pose) const { 41 | double min_dist = kMaxDistance; 42 | const std::vector points = shape_.getCircleDecomposition(ego_pose); 43 | 44 | for (const geom::Vec2& point : points) { 45 | min_dist = std::min(min_dist, std::max(distance(point) - shape_.radius(), 0.0)); 46 | } 47 | 48 | return min_dist; 49 | } 50 | 51 | } // namespace truck::collision 52 | -------------------------------------------------------------------------------- /packages/common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(common) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | add_library(${PROJECT_NAME} SHARED src/format.cpp) 7 | 8 | ament_target_dependencies(${PROJECT_NAME}) 9 | 10 | target_include_directories( 11 | ${PROJECT_NAME} 12 | PUBLIC "$" 13 | "$" 14 | ) 15 | 16 | ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) 17 | # ament_export_dependencies(...) 18 | 19 | install( 20 | TARGETS ${PROJECT_NAME} 21 | EXPORT ${PROJECT_NAME}Targets 22 | ARCHIVE DESTINATION lib 23 | LIBRARY DESTINATION lib 24 | RUNTIME DESTINATION bin 25 | ) 26 | 27 | install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) 28 | 29 | if(BUILD_TESTING) 30 | find_package(ament_cmake_gtest REQUIRED) 31 | add_subdirectory("tests") 32 | endif() 33 | 34 | ament_package() 35 | -------------------------------------------------------------------------------- /packages/common/README.md: -------------------------------------------------------------------------------- 1 | # Common 2 | Just common ¯\_(ツ)_/¯ 3 | -------------------------------------------------------------------------------- /packages/common/include/common/array_as_bit.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "common/math.h" 4 | 5 | #include 6 | #include 7 | 8 | namespace truck { 9 | 10 | /** Binary Indexed Tree (Fenwick Tree) 11 | * 12 | * See https://en.wikipedia.org/wiki/Fenwick_tree 13 | */ 14 | template>> 15 | class ArrayAsBinaryIndexedTree { 16 | public: 17 | ArrayAsBinaryIndexedTree() = delete; 18 | 19 | ArrayAsBinaryIndexedTree(T* data, size_t size) : data_(data), size_(size) {} 20 | 21 | ArrayAsBinaryIndexedTree& Build() noexcept { 22 | for (size_t k = 1; k <= size_; ++k) { 23 | size_t parent_k = k + (k & -k); 24 | if (parent_k <= size_) { 25 | *(data_ + (parent_k - 1)) += *(data_ + (k - 1)); 26 | } 27 | } 28 | return *this; 29 | } 30 | 31 | ArrayAsBinaryIndexedTree& Add(size_t i, const T& val) noexcept { 32 | for (size_t k = i + 1; k <= size_; k += k & -k) { 33 | *(data_ + (k - 1)) += val; 34 | } 35 | return *this; 36 | } 37 | 38 | T Sum(size_t r) const noexcept { 39 | T sum = 0; 40 | for (; r > 0; r -= r & -r) { 41 | sum += *(data_ + (r - 1)); 42 | } 43 | return sum; 44 | } 45 | 46 | T Sum(size_t l, size_t r) const noexcept { return Sum(r) - Sum(l); } 47 | 48 | size_t LowerBound(T sum) const noexcept { 49 | size_t k = 0; 50 | for (size_t b = fls(size_) + 1; b > 0; --b) { 51 | size_t next_k = k + (1 << (b - 1)); 52 | if (next_k <= size_ && *(data_ + (next_k - 1)) < sum) { 53 | k = next_k; 54 | sum -= *(data_ + (next_k - 1)); 55 | } 56 | } 57 | return k; 58 | } 59 | 60 | const T* Data() const noexcept { return data_; } 61 | 62 | T* Data() noexcept { return data_; } 63 | 64 | size_t Size() const noexcept { return size_; } 65 | 66 | private: 67 | T* data_ = nullptr; 68 | size_t size_ = 0; 69 | }; 70 | 71 | } // namespace truck 72 | -------------------------------------------------------------------------------- /packages/common/include/common/array_as_queue.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace truck { 7 | 8 | template 9 | class ArrayAsQueue { 10 | public: 11 | ArrayAsQueue() : head_(nullptr), tail_(nullptr) {} 12 | 13 | ArrayAsQueue(T* arr) : head_(arr), tail_(arr) {} 14 | 15 | void push(const T& value) noexcept { 16 | *tail_ = value; 17 | ++tail_; 18 | } 19 | 20 | void push(T&& value) noexcept { 21 | *tail_ = std::move(value); 22 | ++tail_; 23 | } 24 | 25 | const T& front() const noexcept { return *head_; } 26 | 27 | T& front() noexcept { return *head_; } 28 | 29 | T pop() noexcept { return *(head_++); } 30 | 31 | size_t size() const noexcept { return tail_ - head_; } 32 | 33 | bool empty() const noexcept { return head_ == tail_; } 34 | 35 | T* data() noexcept { return head_; } 36 | 37 | private: 38 | T* head_; 39 | T* tail_; 40 | }; 41 | 42 | } // namespace truck 43 | -------------------------------------------------------------------------------- /packages/common/include/common/exception.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "common/format.h" 8 | 9 | namespace truck { 10 | 11 | class Exception : public std::exception { 12 | public: 13 | Exception() = default; 14 | 15 | Exception(std::string message) : message_(std::move(message)) {} 16 | 17 | Exception(const char* message) : message_(message) {} 18 | 19 | template 20 | Exception(const char* f, const Args&... args) : message_(fmt(f, args...)) {} 21 | 22 | template 23 | Exception& operator<<(const T& value) { 24 | std::ostringstream stream; 25 | stream << value; 26 | message_ += stream.str(); 27 | return *this; 28 | } 29 | 30 | const char* what() const noexcept override { return message_.c_str(); } 31 | 32 | private: 33 | std::string message_; 34 | }; 35 | 36 | #define THROW_EXCEPTION() throw Exception() << __FILE__ << ":" << __LINE__ 37 | 38 | #define VERIFY(expr) \ 39 | [&](auto&& arg) -> decltype(arg) { \ 40 | if (!static_cast(arg)) { \ 41 | THROW_EXCEPTION() << ": '" << #expr; \ 42 | } \ 43 | return std::forward(arg); \ 44 | }(expr) 45 | 46 | #define VERIFY_STREAM(condition, messages) \ 47 | if (!(condition)) { \ 48 | THROW_EXCEPTION() << ": " << #condition << "\n" << messages; \ 49 | } 50 | 51 | #define VERIFY_FMT(condition, ...) \ 52 | if (!(condition)) { \ 53 | THROW_EXCEPTION() << ":" << #condition << "\n" << fmt(__VA_ARGS__); \ 54 | } 55 | 56 | #define FALL(...) THROW_EXCEPTION() << ": " << fmt(__VA_ARGS__); 57 | 58 | } // namespace truck 59 | -------------------------------------------------------------------------------- /packages/common/include/common/format.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace truck { 6 | 7 | std::string fmt(const char* s, ...); 8 | 9 | } // namespace truck 10 | -------------------------------------------------------------------------------- /packages/common/include/common/lock.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "common/exception.h" 4 | 5 | #include 6 | #include 7 | 8 | namespace truck { 9 | 10 | template 11 | class Lockable { 12 | struct Storage { 13 | using Ptr = std::shared_ptr; 14 | 15 | template 16 | Storage(Args&&... args) : mutex{}, data(std::forward(args)...) {} 17 | 18 | std::mutex mutex; 19 | T data; 20 | }; 21 | 22 | class Lock { 23 | public: 24 | Lock() = delete; 25 | 26 | Lock(const Lock&) = delete; 27 | Lock& operator=(const Lock&) = delete; 28 | 29 | Lock(Lock&&) = default; 30 | Lock& operator=(Lock&&) = default; 31 | 32 | ~Lock() { storage_->mutex.unlock(); } 33 | 34 | T* operator->() { return &storage_->data; } 35 | const T* operator->() const { return &storage_->data; } 36 | 37 | T& operator*() { return storage_->data; } 38 | const T& operator*() const { return storage_->data; } 39 | 40 | private: 41 | friend class Lockable; 42 | 43 | Lock(typename Storage::Ptr storage) : storage_(std::move(storage)) { 44 | storage_->mutex.lock(); 45 | } 46 | 47 | typename Storage::Ptr storage_ = nullptr; 48 | }; 49 | 50 | public: 51 | template 52 | explicit Lockable(Args&&... args) : 53 | storage_(std::make_shared(std::forward(args)...)) {} 54 | 55 | auto lock() { return Lock(VERIFY(storage_)); } 56 | 57 | private: 58 | std::shared_ptr storage_ = nullptr; 59 | }; 60 | 61 | template 62 | auto makeLockable(Args&&... args) { 63 | return Lockable(std::forward(args)...); 64 | } 65 | 66 | } // namespace truck 67 | -------------------------------------------------------------------------------- /packages/common/include/common/math.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "common/exception.h" 4 | 5 | #include 6 | #include 7 | 8 | namespace truck { 9 | 10 | template 11 | inline T abs(const T& x) { 12 | return std::abs(x); 13 | } 14 | 15 | template 16 | inline double squared(const T& x) { 17 | return x * x; 18 | } 19 | 20 | template 21 | T clamp(const T& val, const T& low, const T& high) { 22 | return std::clamp(val, low, high); 23 | } 24 | 25 | template 26 | T clamp(const T& val, const T& abs_limit) { 27 | return clamp(val, -abs_limit, abs_limit); 28 | } 29 | 30 | template 31 | inline constexpr int sign(const T& x) { 32 | return (T(0) < x) - (x < T(0)); 33 | } 34 | 35 | template 36 | struct Limits { 37 | Limits() = default; 38 | 39 | Limits(const T& min, const T& max) : min(min), max(max) { VERIFY(min <= max); } 40 | 41 | bool isMet(const T& x) const { return min <= x && x <= max; } 42 | 43 | bool isStrictlyMet(const T& x) const { return min < x && x < max; } 44 | 45 | T clamp(const T& t) const { return truck::clamp(t, min, max); } 46 | 47 | double ratio(const T& t) const { return (clamp(t) - min) / (max - min); } 48 | 49 | T min, max; 50 | }; 51 | 52 | template 53 | Int ceil(Float value) { 54 | return static_cast(std::ceil(value)); 55 | } 56 | 57 | template 58 | Int floor(Float value) { 59 | return static_cast(std::floor(value)); 60 | } 61 | 62 | template 63 | Int round(Float value) { 64 | return static_cast(std::round(value)); 65 | } 66 | 67 | template>> 68 | inline constexpr size_t fls(T value) { 69 | size_t i = 0; 70 | for (; (value >> i) > 0; ++i) { 71 | } 72 | return i; 73 | } 74 | 75 | } // namespace truck 76 | -------------------------------------------------------------------------------- /packages/common/include/common/result.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace truck::common { 7 | 8 | template 9 | class Result { 10 | static_assert(!std::is_same_v, "Value type can not match error type"); 11 | 12 | private: 13 | std::variant state; 14 | 15 | public: 16 | Result() : state(std::in_place_index_t<0>{}) {} 17 | Result(Value value) : state(std::move(value)) {} 18 | Result(Error error) : state(std::move(error)) {} 19 | 20 | operator bool() const { return state.index() == 1; } 21 | 22 | bool empty() const { return state.index() == 0; } 23 | 24 | const auto& get() const& { return std::get<1>(state); } 25 | auto get() && { return std::move(std::get<1>(state)); } 26 | 27 | const auto& operator*() const { return get(); } 28 | const auto* operator->() const { return &get(); } 29 | 30 | const auto& error() const& { return std::get<2>(state); } 31 | 32 | auto error() && { return std::move(std::get<2>(state)); } 33 | 34 | template 35 | static Result ok(Args&&... args) { 36 | return Value(std::forward(args)...); 37 | } 38 | template 39 | static Result fail(Args&&... args) { 40 | return Error(std::forward(args)...); 41 | } 42 | }; 43 | 44 | } // namespace truck::common 45 | -------------------------------------------------------------------------------- /packages/common/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | common 4 | 0.0.0 5 | common 6 | Simagin Denis 7 | MIT 8 | 9 | ament_cmake 10 | 11 | 12 | ament_cmake 13 | 14 | 15 | -------------------------------------------------------------------------------- /packages/common/src/format.cpp: -------------------------------------------------------------------------------- 1 | #include "common/format.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace truck { 7 | 8 | std::string fmt(const char* s, ...) { 9 | va_list args; 10 | va_start(args, s); 11 | 12 | char buffer[4096]; 13 | vsnprintf(buffer, sizeof(buffer), s, args); 14 | 15 | va_end(args); 16 | 17 | return std::string(buffer); 18 | } 19 | 20 | } // namespace truck 21 | -------------------------------------------------------------------------------- /packages/common/tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(${PROJECT_NAME}_tests main.cpp) 2 | target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME}) 3 | 4 | set_tests_properties(${Tests} PROPERTIES TIMEOUT 1) 5 | 6 | target_include_directories( 7 | ${PROJECT_NAME}_tests 8 | PRIVATE "$" 9 | "$" 10 | "$" 11 | ) 12 | -------------------------------------------------------------------------------- /packages/control_proxy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(control_proxy) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(common) 6 | find_package(model REQUIRED) 7 | find_package(rclcpp REQUIRED) 8 | find_package(sensor_msgs REQUIRED) 9 | find_package(std_srvs REQUIRED) 10 | find_package(truck_msgs REQUIRED) 11 | find_package(yaml_cpp_vendor REQUIRED) 12 | 13 | add_executable(${PROJECT_NAME}_node src/main.cpp src/control_proxy_node.cpp) 14 | 15 | ament_target_dependencies( 16 | ${PROJECT_NAME}_node 17 | common 18 | model 19 | rclcpp 20 | sensor_msgs 21 | std_srvs 22 | truck_msgs 23 | yaml_cpp_vendor 24 | ) 25 | 26 | install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME}) 27 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 28 | 29 | ament_package() 30 | -------------------------------------------------------------------------------- /packages/control_proxy/README.md: -------------------------------------------------------------------------------- 1 | # control_proxy 2 | 3 | | [**docs**](../../doc/README.md) | [**packages**](../README.md) | 4 | |---------------------------------|------------------------------| 5 | 6 | ## Overview 7 | The node mixes remote and self-driving control modes: 8 | - Joypad may be used as Red batton, mode switcher and remote control. More information about setting up and using you can find [here](../../doc/remote_control.md). 9 | - If `mode` is `Auto`, node forwards control targets of planner 10 | - If `mode` is `Remote`, you can drive truck with joypad 11 | - Switching to `Remote` bmay be used as Red button 12 | - Watchdog switch `mode` to `Remote` if no commands from planner 13 | - Watchdog switch `mode` to `Off` if lose joypad connection. 14 | 15 | ## Topics 16 | ### Input 17 | - `/motion/command` [[truck_msgs/Control](https://github.com/robotics-laboratory/truck/blob/master/packages/truck_msgs/msg/Control.msg)] - target of trajectory controller 18 | - `/control/input` [[truck_msgs/RemoteControl](https://github.com/robotics-laboratory/truck/blob/master/packages/truck_msgs/msg/RemoteControl.msg)] - remote command topic 19 | 20 | ### Output 21 | - `/control/command` [[truck_interfecase/Control](https://github.com/robotics-laboratory/truck/blob/master/packages/truck_msgs/msg/Control.msg)] - control target command 22 | - `/control/mode` [[truck_interfecase/ControlMode](https://github.com/robotics-laboratory/truck/blob/master/packages/truck_msgs/msg/ControlMode.msg)] - control proxy mode (`Auto`, `Remote`, `Off`) 23 | -------------------------------------------------------------------------------- /packages/control_proxy/launch/control_proxy.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: 3 | name: "simulation" 4 | default: "false" 5 | - node: 6 | pkg: "control_proxy" 7 | exec: "control_proxy_node" 8 | name: "control_proxy_node" 9 | output: "log" 10 | param: 11 | # use /clock topic time 12 | - { name: "use_sim_time", value: "$(var simulation)" } 13 | # path to truck model config 14 | - { name: "model_config", value: "$(find-pkg-share model)/config/model.yaml" } 15 | # timers period (milliseconds) 16 | - { name: "mode_period", value: 200 } 17 | - { name: "watchdog_period", value: 20 } 18 | # set mode to `off` if no commands from remote during this time (milliseconds) 19 | - { name: "remote_timeout", value: 200 } 20 | # set mode to `remote` if no commands from planner during this time (milliseconds) 21 | - { name: "control_timeout", value: 150 } 22 | -------------------------------------------------------------------------------- /packages/control_proxy/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | control_proxy 5 | 0.1.0 6 | control_proxy 7 | Simagin Denis 8 | MIT 9 | 10 | ament_cmake 11 | ros2launch 12 | 13 | common 14 | model 15 | rclcpp 16 | sensor_msgs 17 | std_msgs 18 | truck_msgs 19 | yaml_cpp_vendor 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /packages/control_proxy/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "control_proxy_node.h" 4 | #include "rclcpp/rclcpp.hpp" 5 | 6 | int main(int argc, char* argv[]) { 7 | rclcpp::init(argc, argv); 8 | rclcpp::spin(std::make_shared()); 9 | rclcpp::shutdown(); 10 | return 0; 11 | } 12 | -------------------------------------------------------------------------------- /packages/fastgrid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(fastgrid) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(common REQUIRED) 6 | find_package(geom REQUIRED) 7 | find_package(OpenCV REQUIRED) 8 | 9 | add_library( 10 | ${PROJECT_NAME} SHARED src/distance_transform.cpp src/grid.cpp 11 | src/manhattan_distance.cpp src/draw.cpp 12 | ) 13 | 14 | ament_target_dependencies(${PROJECT_NAME} common geom OpenCV) 15 | 16 | target_include_directories( 17 | ${PROJECT_NAME} 18 | PUBLIC "$" 19 | "$" 20 | ) 21 | 22 | ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) 23 | ament_export_dependencies(common geom OpenCV) 24 | 25 | install( 26 | TARGETS ${PROJECT_NAME} 27 | EXPORT ${PROJECT_NAME} 28 | ARCHIVE DESTINATION lib 29 | LIBRARY DESTINATION lib 30 | RUNTIME DESTINATION bin 31 | ) 32 | 33 | install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) 34 | 35 | if(BUILD_TESTING) 36 | add_subdirectory("test") 37 | # add_subdirectory("benchmark") 38 | endif() 39 | 40 | ament_package() 41 | -------------------------------------------------------------------------------- /packages/fastgrid/benchmark/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(ament_cmake_google_benchmark REQUIRED) 2 | find_package(OpenCV REQUIRED) 3 | 4 | ament_add_google_benchmark(${PROJECT_NAME}_benchmarks main.cpp TIMEOUT 100) 5 | target_link_libraries(${PROJECT_NAME}_benchmarks ${PROJECT_NAME} ${OpenCV_LIBS}) 6 | 7 | target_include_directories( 8 | ${PROJECT_NAME}_benchmarks 9 | PRIVATE "$" 10 | "$" 11 | "$" 12 | ) 13 | -------------------------------------------------------------------------------- /packages/fastgrid/include/fastgrid/distance_transform.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "fastgrid/grid.h" 4 | #include "fastgrid/holder.h" 5 | 6 | namespace truck::fastgrid { 7 | 8 | void distanceTransformApprox3(const U8Grid& input, S32Grid& buf, F32Grid& out) noexcept; 9 | 10 | void distanceTransformApprox3(const U8Grid& input, F32Grid& out) noexcept; 11 | 12 | F32GridHolder distanceTransformApprox3(const U8Grid& input) noexcept; 13 | 14 | void distanceTransformApprox5(const U8Grid& input, S32Grid& buf, F32Grid& out) noexcept; 15 | 16 | void distanceTransformApprox5(const U8Grid& input, F32Grid& out) noexcept; 17 | 18 | F32GridHolder distanceTransformApprox5(const U8Grid& input) noexcept; 19 | 20 | } // namespace truck::fastgrid 21 | -------------------------------------------------------------------------------- /packages/fastgrid/include/fastgrid/draw.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "fastgrid/grid.h" 4 | #include "fastgrid/holder.h" 5 | 6 | #include "geom/complex_polygon.h" 7 | #include "geom/polygon.h" 8 | 9 | namespace truck::fastgrid { 10 | 11 | void draw(const geom::Polygon& poly, U8Grid& grid); 12 | 13 | void draw(const geom::ComplexPolygon& complex_poly, U8Grid& grid); 14 | 15 | } // namespace truck::fastgrid 16 | -------------------------------------------------------------------------------- /packages/fastgrid/include/fastgrid/manhattan_distance.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "fastgrid/grid.h" 4 | #include "fastgrid/holder.h" 5 | 6 | #include "common/array_as_queue.h" 7 | 8 | #include "geom/vector.h" 9 | 10 | namespace truck::fastgrid { 11 | 12 | void manhattanDistance( 13 | const F32Grid& distance_transform, ArrayAsQueue& queue, float eps, 14 | F32Grid& manhattan_distance); 15 | 16 | F32GridHolder manhattanDistance( 17 | const F32Grid& distance_transform, ArrayAsQueue& queue, float eps); 18 | 19 | void manhattanDistance( 20 | const F32Grid& distance_transform, const geom::Vec2& source, float eps, int* queue_buf, 21 | F32Grid& manhattan_distance); 22 | 23 | void manhattanDistance( 24 | const F32Grid& distance_transform, const geom::Vec2& source, float eps, 25 | F32Grid& manhattan_distance); 26 | 27 | F32GridHolder manhattanDistance( 28 | const F32Grid& distance_transform, const geom::Vec2& source, float eps); 29 | 30 | } // namespace truck::fastgrid 31 | -------------------------------------------------------------------------------- /packages/fastgrid/include/fastgrid/misc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace truck::fastgrid { 6 | 7 | template 8 | void setTo(T* arr, int size, const T& val) { 9 | std::fill(arr, arr + size, val); 10 | } 11 | 12 | } // namespace truck::fastgrid 13 | -------------------------------------------------------------------------------- /packages/fastgrid/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | fastgrid 5 | 0.0.0 6 | fastgrid 7 | Nikita Romanov 8 | MIT 9 | 10 | ament_cmake 11 | common 12 | geom 13 | OpenCV 14 | 15 | -------------------------------------------------------------------------------- /packages/fastgrid/src/grid.cpp: -------------------------------------------------------------------------------- 1 | #include "fastgrid/grid.h" 2 | 3 | namespace truck::fastgrid { 4 | 5 | bool operator==(const Size& a, const Size& b) noexcept { 6 | return a.width == b.width && a.height == b.height; 7 | } 8 | 9 | bool operator!=(const Size& a, const Size& b) noexcept { return !(a == b); } 10 | 11 | std::ostream& operator<<(std::ostream& out, const Size& index) noexcept { 12 | return out << index.width << "x" << index.height; 13 | } 14 | 15 | bool operator==(const Index& a, const Index& b) noexcept { return a.x == b.x && a.y == b.y; } 16 | 17 | bool operator!=(const Index& a, const Index& b) noexcept { return !(a == b); } 18 | 19 | std::ostream& operator<<(std::ostream& out, const Index& index) noexcept { 20 | return out << index.x << ":" << index.y; 21 | } 22 | 23 | } // namespace truck::fastgrid 24 | -------------------------------------------------------------------------------- /packages/fastgrid/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(ament_cmake_gtest REQUIRED) 2 | 3 | ament_add_gtest(${PROJECT_NAME}_tests main.cpp) 4 | target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME}) 5 | set_tests_properties(${Tests} PROPERTIES TIMEOUT 10) 6 | 7 | target_include_directories( 8 | ${PROJECT_NAME}_tests 9 | PRIVATE "$" 10 | "$" 11 | "$" 12 | ) 13 | -------------------------------------------------------------------------------- /packages/geom/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(geom) 3 | 4 | set(EIGEN3_DIR /usr/include/eigen3) 5 | 6 | find_package(ament_cmake REQUIRED) 7 | find_package(common REQUIRED) 8 | find_package(geometry_msgs REQUIRED) 9 | find_package(nav_msgs REQUIRED) 10 | find_package(tf2) 11 | find_package(CGAL REQUIRED) 12 | find_package(Boost REQUIRED) 13 | find_package(Eigen3 REQUIRED) 14 | 15 | add_library( 16 | ${PROJECT_NAME} SHARED 17 | src/angle.cpp 18 | src/angle_vector.cpp 19 | src/arc.cpp 20 | src/bezier.cpp 21 | src/bounding_box.cpp 22 | src/circle.cpp 23 | src/complex_polygon.cpp 24 | src/distance.cpp 25 | src/intersection.cpp 26 | src/line.cpp 27 | src/motion_state.cpp 28 | src/msg.cpp 29 | src/polygon.cpp 30 | src/polyline.cpp 31 | src/pose.cpp 32 | src/segment.cpp 33 | src/transform.cpp 34 | src/vector.cpp 35 | src/vector3.cpp 36 | ) 37 | 38 | ament_target_dependencies( 39 | ${PROJECT_NAME} 40 | common 41 | geometry_msgs 42 | nav_msgs 43 | tf2 44 | CGAL 45 | Boost 46 | Eigen3 47 | ) 48 | 49 | include_directories(${EIGEN3_DIR}) 50 | 51 | target_include_directories( 52 | ${PROJECT_NAME} 53 | PUBLIC "$" 54 | "$" 55 | ) 56 | 57 | ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) 58 | 59 | ament_export_dependencies( 60 | common 61 | geometry_msgs 62 | nav_msgs 63 | tf2 64 | CGAL 65 | Boost 66 | Eigen3 67 | ) 68 | 69 | install( 70 | TARGETS ${PROJECT_NAME} 71 | EXPORT ${PROJECT_NAME} 72 | ARCHIVE DESTINATION lib 73 | LIBRARY DESTINATION lib 74 | RUNTIME DESTINATION bin 75 | ) 76 | 77 | install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) 78 | 79 | if(BUILD_TESTING) 80 | find_package(ament_cmake_gtest REQUIRED) 81 | add_subdirectory("test") 82 | endif() 83 | 84 | ament_package() 85 | -------------------------------------------------------------------------------- /packages/geom/include/geom/arc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/common.h" 4 | #include "geom/segment.h" 5 | #include "geom/vector.h" 6 | #include "geom/polyline.h" 7 | 8 | #include "common/exception.h" 9 | 10 | #include 11 | 12 | namespace truck::geom { 13 | 14 | struct Arc { 15 | Arc(const Vec2& center, double radius, const Vec2 begin, Angle delta) : 16 | center(center), radius(radius), begin(begin), delta(delta) { 17 | VERIFY(radius > 0); 18 | } 19 | 20 | int ccw() const { return delta >= Angle::zero() ? 1 : -1; }; 21 | 22 | double curv() const { return ccw() / radius; } 23 | 24 | double len() const { return radius * abs(delta).radians(); } 25 | 26 | Poses trace(double step) const; 27 | 28 | Vec2 center; 29 | double radius; 30 | Vec2 begin; 31 | Angle delta; 32 | }; 33 | 34 | std::ostream& operator<<(std::ostream& os, const Arc& arc); 35 | 36 | std::variant tryBuildArc( 37 | const Pose& from, const Vec2& to, double eps = 1e-3); 38 | 39 | } // namespace truck::geom 40 | -------------------------------------------------------------------------------- /packages/geom/include/geom/bezier.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/motion_state.h" 4 | #include "geom/vector.h" 5 | 6 | namespace truck::geom { 7 | 8 | MotionStates bezier1(const Vec2& p0, const Vec2& p1, size_t n); 9 | MotionStates bezier1(const Vec2& p0, const Vec2& p1, double step); 10 | 11 | MotionStates bezier2(const Vec2& p0, const Vec2& p1, const Vec2& p2, size_t n); 12 | MotionStates bezier2(const Vec2& p0, const Vec2& p1, const Vec2& p2, double step); 13 | 14 | MotionStates bezier3(const Vec2& p0, const Vec2& p1, const Vec2& p2, const Vec2& p3, size_t n); 15 | MotionStates bezier3(const Vec2& p0, const Vec2& p1, const Vec2& p2, const Vec2& p3, double step); 16 | 17 | } // namespace truck::geom 18 | -------------------------------------------------------------------------------- /packages/geom/include/geom/boost.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/boost/box.h" 4 | #include "geom/boost/linestring.h" 5 | #include "geom/boost/point.h" 6 | #include "geom/boost/polygon.h" 7 | #include "geom/boost/ring.h" 8 | #include "geom/boost/segment.h" 9 | -------------------------------------------------------------------------------- /packages/geom/include/geom/boost/box.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/bounding_box.h" 4 | #include "geom/boost/point.h" 5 | 6 | #include 7 | #include 8 | 9 | BOOST_GEOMETRY_REGISTER_BOX(truck::geom::BoundingBox, truck::geom::Vec2, min, max) 10 | -------------------------------------------------------------------------------- /packages/geom/include/geom/boost/linestring.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/polyline.h" 4 | #include "geom/boost/point.h" 5 | 6 | #include 7 | #include 8 | 9 | BOOST_GEOMETRY_REGISTER_LINESTRING(truck::geom::Polyline) 10 | -------------------------------------------------------------------------------- /packages/geom/include/geom/boost/point.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/vector.h" 4 | #include "geom/vector3.h" 5 | 6 | #include 7 | #include 8 | 9 | BOOST_GEOMETRY_REGISTER_POINT_2D(truck::geom::Vec2, double, cs::cartesian, x, y) 10 | BOOST_GEOMETRY_REGISTER_POINT_3D(truck::geom::Vec3, double, cs::cartesian, x, y, z) 11 | -------------------------------------------------------------------------------- /packages/geom/include/geom/boost/polygon.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/complex_polygon.h" 4 | #include "geom/boost/point.h" 5 | #include "geom/boost/ring.h" 6 | 7 | #include 8 | #include 9 | 10 | namespace boost { 11 | namespace geometry { 12 | namespace traits { 13 | 14 | using namespace truck::geom; 15 | 16 | template<> 17 | struct tag { 18 | typedef polygon_tag type; 19 | }; 20 | 21 | template<> 22 | struct ring_const_type { 23 | typedef const Polygon& type; 24 | }; 25 | 26 | template<> 27 | struct ring_mutable_type { 28 | typedef Polygon& type; 29 | }; 30 | 31 | template<> 32 | struct interior_const_type { 33 | typedef const std::vector& type; 34 | }; 35 | 36 | template<> 37 | struct interior_mutable_type { 38 | typedef std::vector& type; 39 | }; 40 | 41 | template<> 42 | struct exterior_ring { 43 | static Polygon& get(ComplexPolygon& p) { return (p.outer); } 44 | static const Polygon& get(const ComplexPolygon& p) { return (p.outer); } 45 | }; 46 | 47 | template<> 48 | struct interior_rings { 49 | static std::vector& get(ComplexPolygon& p) { return p.inners; } 50 | static const std::vector& get(const ComplexPolygon& p) { return p.inners; } 51 | }; 52 | 53 | } // namespace traits 54 | } // namespace geometry 55 | } // namespace boost 56 | -------------------------------------------------------------------------------- /packages/geom/include/geom/boost/ring.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/polygon.h" 4 | #include "geom/boost/point.h" 5 | 6 | #include 7 | #include 8 | 9 | BOOST_GEOMETRY_REGISTER_RING(truck::geom::Polygon) 10 | -------------------------------------------------------------------------------- /packages/geom/include/geom/boost/segment.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/segment.h" 4 | #include "geom/boost/point.h" 5 | 6 | #include 7 | #include 8 | 9 | BOOST_GEOMETRY_REGISTER_SEGMENT(truck::geom::Segment, truck::geom::Vec2, begin, end) 10 | -------------------------------------------------------------------------------- /packages/geom/include/geom/bounding_box.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/vector.h" 4 | 5 | #include 6 | 7 | namespace truck::geom { 8 | 9 | struct BoundingBox { 10 | BoundingBox(const Vec2& v) { 11 | min = v; 12 | max = v; 13 | } 14 | 15 | BoundingBox(const Vec2& a, const Vec2& b) { 16 | min.x = std::min(a.x, b.x); 17 | min.y = std::min(a.y, b.y); 18 | max.x = std::max(a.x, b.x); 19 | max.y = std::max(a.y, b.y); 20 | } 21 | 22 | BoundingBox& extend(const Vec2& v) noexcept; 23 | BoundingBox& extend(double margin) noexcept; 24 | 25 | Vec2 min, max; 26 | }; 27 | 28 | inline BoundingBox extend(BoundingBox& box, const Vec2& v) noexcept { return box.extend(v); } 29 | 30 | inline BoundingBox extend(BoundingBox& box, double margin) noexcept { return box.extend(margin); } 31 | 32 | } // namespace truck::geom 33 | -------------------------------------------------------------------------------- /packages/geom/include/geom/circle.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/vector.h" 4 | 5 | #include 6 | 7 | namespace truck::geom { 8 | 9 | struct Circle { 10 | Vec2 center; 11 | double radius; 12 | }; 13 | 14 | bool near(const Circle& a, const Circle& b, double eps = 0) noexcept; 15 | 16 | std::ostream& operator<<(std::ostream& out, const Circle& c) noexcept; 17 | 18 | } // namespace truck::geom 19 | -------------------------------------------------------------------------------- /packages/geom/include/geom/common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "common/math.h" 5 | 6 | namespace truck::geom { 7 | 8 | inline bool equal(double a, double b, double eps = 0) noexcept { return std::abs(a - b) <= eps; } 9 | inline double interpolate(double a, double b, double t) noexcept { 10 | VERIFY(0 <= t && t <= 1); 11 | return (1 - t) * a + t * b; 12 | } 13 | } // namespace truck::geom 14 | -------------------------------------------------------------------------------- /packages/geom/include/geom/complex_polygon.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/polygon.h" 4 | 5 | #include 6 | 7 | namespace truck::geom { 8 | 9 | struct ComplexPolygon { 10 | ComplexPolygon() = default; 11 | 12 | ComplexPolygon(Polygon outer) : outer(std::move(outer)) {} 13 | 14 | ComplexPolygon(Polygon outer, Polygons inners) : 15 | outer(std::move(outer)), inners(std::move(inners)) {} 16 | 17 | std::vector triangles() const noexcept; 18 | Segments segments() const noexcept; 19 | 20 | Polygon outer; 21 | Polygons inners; 22 | }; 23 | 24 | using ComplexPolygons = std::vector; 25 | 26 | } // namespace truck::geom 27 | -------------------------------------------------------------------------------- /packages/geom/include/geom/distance.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/line.h" 4 | #include "geom/segment.h" 5 | #include "geom/vector.h" 6 | #include "geom/motion_state.h" 7 | 8 | namespace truck::geom { 9 | 10 | double distanceSq(const Vec2& a, const Vec2& b) noexcept; 11 | 12 | double distance(const Vec2& a, const Vec2& b) noexcept; 13 | 14 | double distanceSq(const Vec2& p, const Segment& s) noexcept; 15 | 16 | double distance(const Vec2& p, const Segment& s) noexcept; 17 | 18 | double denormalizedDistance(const Line& l, const Vec2& p) noexcept; 19 | 20 | double denormalizedDistance(const Vec2& p, const Line& l) noexcept; 21 | 22 | double distance(const Line& l, const Vec2& p) noexcept; 23 | 24 | double distance(const Vec2& p, const Line& l) noexcept; 25 | 26 | double distanceSq(const MotionState& a, const MotionState& b) noexcept; 27 | 28 | double distance(const MotionState& a, const MotionState& b) noexcept; 29 | 30 | double distance(const Segment& a, const Segment& b) noexcept; 31 | 32 | } // namespace truck::geom 33 | -------------------------------------------------------------------------------- /packages/geom/include/geom/intersection.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/ray.h" 4 | #include "geom/line.h" 5 | #include "geom/segment.h" 6 | #include "geom/polygon.h" 7 | #include "geom/vector.h" 8 | 9 | #include 10 | 11 | namespace truck::geom { 12 | 13 | bool intersect(const Segment& seg1, const Segment& seg2, const double eps = 1e-4) noexcept; 14 | 15 | bool intersect(const Polygon& polygon, const Segment& seg, const double eps = 1e-4) noexcept; 16 | 17 | std::optional intersect(const Line& l1, const Line& l2, const double eps = 1e-4) noexcept; 18 | 19 | std::optional intersect(const Ray& ray, const Segment& segment, double precision) noexcept; 20 | 21 | } // namespace truck::geom 22 | -------------------------------------------------------------------------------- /packages/geom/include/geom/line.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/common.h" 4 | #include "geom/vector.h" 5 | 6 | #include 7 | 8 | namespace truck::geom { 9 | 10 | // ax + by + c = 0 11 | struct Line { 12 | Line(double a, double b, double c) : a(a), b(b), c(c) {} 13 | 14 | Line(const Vec2& norm, double c) : a(norm.x), b(norm.y), c(c) {} 15 | 16 | Vec2 normal() const noexcept { return {a, b}; } 17 | 18 | static Line fromTwoPoints(const Vec2& p1, const Vec2& p2) noexcept { 19 | Vec2 norm{p2.y - p1.y, p1.x - p2.x}; 20 | return Line(norm, -dot(p1, norm)); 21 | } 22 | 23 | static Line fromPointAndNormal(const Vec2& p, const Vec2& norm) noexcept { 24 | return Line(norm, -dot(p, norm)); 25 | } 26 | 27 | static Line fromPointAndCollinear(const Vec2& p, const Vec2& col) noexcept { 28 | return fromPointAndNormal(p, Vec2{col.y, -col.x}); 29 | } 30 | 31 | double a, b, c; 32 | }; 33 | 34 | bool equal(const Line& a, const Line& b, double eps) noexcept; 35 | 36 | std::ostream& operator<<(std::ostream& out, const Line& l) noexcept; 37 | 38 | } // namespace truck::geom 39 | -------------------------------------------------------------------------------- /packages/geom/include/geom/localization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/pose.h" 4 | 5 | namespace truck::geom { 6 | 7 | struct Localization { 8 | Pose pose; 9 | double velocity; 10 | }; 11 | 12 | } // namespace truck::geom 13 | -------------------------------------------------------------------------------- /packages/geom/include/geom/motion_state.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/pose.h" 4 | #include "geom/common.h" 5 | #include 6 | 7 | namespace truck::geom { 8 | 9 | struct MotionState { 10 | Vec2 pos; 11 | AngleVec2 dir; 12 | double curvature = 0; 13 | 14 | Pose pose() const { return Pose{.pos = pos, .dir = dir}; } 15 | }; 16 | 17 | using MotionStates = std::vector; 18 | 19 | Poses toPoses(const MotionStates& states); 20 | 21 | struct MotionStateLinearInterpolator { 22 | MotionState operator()(const MotionState& from, const MotionState& to, double t) const { 23 | return { 24 | .pos = interpolate(from.pos, to.pos, t), 25 | .dir = interpolate(from.dir, to.dir, t), 26 | .curvature = interpolate(from.curvature, to.curvature, t), 27 | }; 28 | } 29 | 30 | double operator()(double from, const double to, double t) const { 31 | return interpolate(from, to, t); 32 | } 33 | }; 34 | 35 | } // namespace truck::geom 36 | -------------------------------------------------------------------------------- /packages/geom/include/geom/msg.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/localization.h" 4 | #include "geom/pose.h" 5 | #include "geom/transform.h" 6 | #include "geom/vector.h" 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | namespace truck::geom { 20 | 21 | Vec2 toVec2(const geometry_msgs::msg::Point& p); 22 | 23 | Vec2 toVec2(const geometry_msgs::msg::PointStamped& p); 24 | 25 | Vec2 toVec2(const geometry_msgs::msg::PoseStamped& p); 26 | 27 | Vec2 toVec2(const geometry_msgs::msg::Pose& p); 28 | 29 | Vec2 toVec2(const nav_msgs::msg::Odometry& odom); 30 | 31 | Angle toAngle(const geometry_msgs::msg::Quaternion& q); 32 | 33 | Angle toYawAngle(const geometry_msgs::msg::Quaternion& q); 34 | 35 | AngleVec2 toYawDir(const geometry_msgs::msg::Quaternion& q); 36 | 37 | Pose toPose(const geometry_msgs::msg::Pose& p); 38 | 39 | Pose toPose(const nav_msgs::msg::Odometry& odom); 40 | 41 | Vec2 toVec2(const geometry_msgs::msg::Vector3& v); 42 | 43 | Transform toTransform(const geometry_msgs::msg::Transform& t); 44 | 45 | Transform toTransform(const geometry_msgs::msg::TransformStamped& t); 46 | 47 | Localization toLocalization(const nav_msgs::msg::Odometry& odom); 48 | 49 | namespace msg { 50 | 51 | geometry_msgs::msg::Quaternion toQuaternion(const Angle& a); 52 | 53 | geometry_msgs::msg::Quaternion toQuaternion(const AngleVec2& v); 54 | 55 | geometry_msgs::msg::Point toPoint(const Vec2& v); 56 | 57 | geometry_msgs::msg::Point toPoint(const Pose& p); 58 | 59 | geometry_msgs::msg::Pose toPose(const Pose& p); 60 | 61 | geometry_msgs::msg::PoseStamped toPoseStamped(const std_msgs::msg::Header& header, const Pose& p); 62 | 63 | } // namespace msg 64 | } // namespace truck::geom 65 | -------------------------------------------------------------------------------- /packages/geom/include/geom/polygon.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/bounding_box.h" 4 | #include "geom/segment.h" 5 | #include "geom/triangle.h" 6 | #include "geom/vector.h" 7 | 8 | #include 9 | 10 | namespace truck::geom { 11 | 12 | enum class Orientation : int8_t { kCounterClockwise = 1, kClockwise = -1 }; 13 | 14 | /** 2D Polygon 15 | * 16 | * The following assumptions are imposed on the geom::Polygon object: 17 | * 1. geom::Polygon is a simple polygon (that is, there are no self-intersections in it). 18 | * 2. the polygon is not degenerate (that is, it has at least 3 vertices). 19 | * 3. the vertices of geom::Polygon do not contain 3 consecutive vertices lying on the same line. 20 | * 4. the list of vertices does not require explicit closure (that is, for the ABC polygon, the list 21 | * of vertices has the form [A, B, C], and not [A, B, C, A]). 22 | */ 23 | 24 | struct Polygon : public std::vector { 25 | using vector::vector; 26 | using vector::operator=; 27 | 28 | std::vector triangles() const noexcept; 29 | bool isConvex() const noexcept; 30 | Orientation orientation() const noexcept; 31 | Segments segments() const noexcept; 32 | }; 33 | 34 | using Polygons = std::vector; 35 | 36 | Polygon clip( 37 | const Polygon& clip_polygon, const Polygon& subject_polygon, const double eps = 1e-4) noexcept; 38 | 39 | BoundingBox makeBoundingBox(const Polygon& polygon) noexcept; 40 | 41 | } // namespace truck::geom 42 | -------------------------------------------------------------------------------- /packages/geom/include/geom/polyline.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/uniform_stepper.h" 4 | #include "geom/vector.h" 5 | 6 | #include 7 | 8 | namespace truck::geom { 9 | 10 | struct Polyline final : public std::vector { 11 | using vector::vector; 12 | 13 | using vector::operator=; 14 | 15 | UniformStepper ubegin() const noexcept; 16 | 17 | UniformStepper ubegin(double step_length) const noexcept; 18 | 19 | UniformStepper uend() const noexcept; 20 | 21 | double len() const noexcept; 22 | }; 23 | 24 | Polyline toSpline(const Polyline& polyline, double step, size_t degree) noexcept; 25 | 26 | Polyline toLinearSpline(const Polyline& polyline, double step) noexcept; 27 | 28 | Polyline toQuadraticSpline(const Polyline& polyline, double step) noexcept; 29 | 30 | } // namespace truck::geom 31 | -------------------------------------------------------------------------------- /packages/geom/include/geom/pose.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/angle_vector.h" 4 | #include "geom/vector.h" 5 | #include "geom/common.h" 6 | 7 | #include 8 | #include 9 | 10 | namespace truck::geom { 11 | 12 | /* 13 | * Position of rigid body in 2D space. 14 | * 15 | * The pose is a combination of position (translation) and orientation (rotation). 16 | */ 17 | 18 | struct Pose { 19 | constexpr operator Vec2() const { return pos; } 20 | 21 | Vec2 pos{}; 22 | AngleVec2 dir{}; 23 | }; 24 | 25 | std::ostream& operator<<(std::ostream& out, const Pose& pose) noexcept; 26 | 27 | Pose interpolate(const Pose& a, const Pose& b, double t) noexcept; 28 | 29 | using Poses = std::vector; 30 | 31 | } // namespace truck::geom 32 | -------------------------------------------------------------------------------- /packages/geom/include/geom/ray.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/angle_vector.h" 4 | #include "geom/vector.h" 5 | 6 | namespace truck::geom { 7 | 8 | struct Ray { 9 | Ray(const Vec2& origin, const AngleVec2& dir) : origin(origin), dir(dir) {} 10 | 11 | Vec2 origin; 12 | AngleVec2 dir; 13 | }; 14 | 15 | } // namespace truck::geom 16 | -------------------------------------------------------------------------------- /packages/geom/include/geom/segment.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/pose.h" 4 | #include "geom/vector.h" 5 | 6 | namespace truck::geom { 7 | 8 | struct Segment { 9 | Segment(const Vec2& begin, const Vec2& end) : begin(begin), end(end) {} 10 | 11 | operator Vec2() const noexcept { return end - begin; } 12 | 13 | double lenSq() const noexcept { return geom::lenSq(end - begin); } 14 | 15 | double len() const { return geom::len(end - begin); } 16 | 17 | Vec2 dir() const { return (end - begin).unit(); } 18 | 19 | Vec2 pos(double t) const noexcept; 20 | 21 | Poses trace(double step) const noexcept; 22 | 23 | Vec2 begin; 24 | Vec2 end; 25 | }; 26 | 27 | geom::Vec2 projection(const geom::Vec2& point, const geom::Segment& segment) noexcept; 28 | 29 | using Segments = std::vector; 30 | 31 | } // namespace truck::geom 32 | -------------------------------------------------------------------------------- /packages/geom/include/geom/swap.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace truck::geom { 4 | 5 | inline void swap(Angle& a, Angle& b) noexcept { 6 | const auto a_rad = a.radians(); 7 | a = Angle(b.radians()); 8 | b = Angle(a_rad); 9 | } 10 | 11 | } // namespace truck::geom 12 | -------------------------------------------------------------------------------- /packages/geom/include/geom/test/equal_assert.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "geom/common.h" 6 | 7 | namespace truck::geom { 8 | 9 | template 10 | ::testing::AssertionResult geomEqual(const T& a, const T& b, double eps = 0) { 11 | if (equal(a, b, eps)) { 12 | return ::testing::AssertionSuccess(); 13 | } else { 14 | return ::testing::AssertionFailure() << a << " != " << b << " eps=" << eps; 15 | } 16 | } 17 | 18 | template 19 | ::testing::AssertionResult geomNotEqual(const T& a, const T& b, double eps = 0) { 20 | if (!equal(a, b, eps)) { 21 | return ::testing::AssertionSuccess(); 22 | } else { 23 | return ::testing::AssertionFailure() << a << " == " << b << " eps= " << eps; 24 | } 25 | } 26 | 27 | } // namespace truck::geom 28 | 29 | #define ASSERT_GEOM_EQUAL(...) EXPECT_TRUE(::truck::geom::geomEqual(__VA_ARGS__)) 30 | #define ASSERT_GEOM_NOT_EQUAL(...) EXPECT_TRUE(::truck::geom::geomNotEqual(__VA_ARGS__)) 31 | -------------------------------------------------------------------------------- /packages/geom/include/geom/transform.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/angle.h" 4 | #include "geom/vector.h" 5 | #include "geom/pose.h" 6 | 7 | #include 8 | 9 | #include 10 | 11 | namespace truck::geom { 12 | 13 | /* 14 | * Transform of rigid body in 2D space. 15 | * 16 | * The transform is a combination of translation and rotation. 17 | * Pose of rigid body can be represented as a transform applied to the origin. 18 | */ 19 | 20 | class Transform { 21 | public: 22 | Transform() = default; 23 | 24 | Transform(Vec2 t, Angle a); 25 | Transform(Vec2 t, AngleVec2 r); 26 | 27 | Transform(const tf2::Transform& tf); 28 | 29 | Vec2 apply(Vec2 v) const; 30 | Pose apply(Pose p) const; 31 | 32 | Vec2 operator()(Vec2 v) const; 33 | Pose operator()(Pose p) const; 34 | 35 | const Vec2& t() const { return translation_; } 36 | const AngleVec2& r() const { return rotation_; } 37 | 38 | Transform inv() const; 39 | 40 | private: 41 | Vec2 translation_ = {}; 42 | AngleVec2 rotation_ = {}; 43 | }; 44 | 45 | std::ostream& operator<<(std::ostream& out, const Transform& transform) noexcept; 46 | 47 | } // namespace truck::geom 48 | -------------------------------------------------------------------------------- /packages/geom/include/geom/triangle.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/vector.h" 4 | 5 | namespace truck::geom { 6 | 7 | struct Triangle { 8 | Triangle(const Vec2& p1, const Vec2& p2, const Vec2& p3) : p1(p1), p2(p2), p3(p3) {} 9 | 10 | Vec2 p1; 11 | Vec2 p2; 12 | Vec2 p3; 13 | }; 14 | 15 | } // namespace truck::geom 16 | -------------------------------------------------------------------------------- /packages/geom/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | geom 4 | 0.1.0 5 | Geometry library 6 | Simagin Denis 7 | MIT 8 | 9 | ament_cmake 10 | common 11 | nav_msgs 12 | geometry_msgs 13 | CGAL 14 | Boost 15 | Eigen3 16 | 17 | -------------------------------------------------------------------------------- /packages/geom/src/angle.cpp: -------------------------------------------------------------------------------- 1 | #include "geom/angle.h" 2 | 3 | #include 4 | 5 | namespace truck::geom { 6 | 7 | std::ostream& operator<<(std::ostream& out, const Angle& angle) noexcept { 8 | return out << std::fixed << std::setprecision(2) << angle.degrees() << "'"; 9 | } 10 | 11 | bool equal(Angle a, Angle b, double eps) noexcept { 12 | return abs((a - b)._mPI_PI()).radians() <= eps; 13 | } 14 | 15 | } // namespace truck::geom 16 | -------------------------------------------------------------------------------- /packages/geom/src/angle_vector.cpp: -------------------------------------------------------------------------------- 1 | #include "geom/angle_vector.h" 2 | 3 | #include "common/exception.h" 4 | #include "geom/common.h" 5 | 6 | namespace truck::geom { 7 | 8 | bool equal(const AngleVec2& a, const AngleVec2& b, double eps) noexcept { 9 | return equal(a.x(), b.x(), eps) && equal(a.y(), b.y(), eps); 10 | } 11 | 12 | AngleVec2 interpolate(const AngleVec2& a, const AngleVec2& b, double t) noexcept { 13 | VERIFY(0 <= t && t <= 1); 14 | return {a.angle() + t * (b.angle() - a.angle())}; 15 | } 16 | 17 | std::ostream& operator<<(std::ostream& out, const AngleVec2& a) noexcept { 18 | return out << "[" << a.vec() << ": " << a.angle() << "]"; 19 | } 20 | 21 | } // namespace truck::geom 22 | -------------------------------------------------------------------------------- /packages/geom/src/arc.cpp: -------------------------------------------------------------------------------- 1 | #include "geom/arc.h" 2 | #include "geom/common.h" 3 | 4 | #include "common/exception.h" 5 | #include "common/math.h" 6 | 7 | #include 8 | 9 | namespace truck::geom { 10 | 11 | std::variant tryBuildArc( 12 | const Pose& from, const Vec2& to, double eps) { 13 | const Vec2 chord = to - from.pos; 14 | const double len = chord.len(); 15 | 16 | if (len < eps) { 17 | return std::monostate(); 18 | } 19 | 20 | const Vec2 chord_dir = chord / len; 21 | 22 | const double cross_prod = cross(from.dir, chord_dir); 23 | const double dot_prod = dot(from.dir, chord_dir); 24 | 25 | if (equal(cross_prod, 0.0, eps)) { 26 | if (equal(dot_prod, 1.0, eps)) { 27 | return Segment{from.pos, to}; 28 | } 29 | 30 | return std::monostate(); 31 | } 32 | 33 | const double radius = len / (2 * std::abs(cross_prod)); 34 | const Vec2 radius_dir = cross_prod > 0 ? from.dir.left() : from.dir.right(); 35 | 36 | return Arc(from.pos + radius * radius_dir, radius, -radius_dir, 2 * atan(cross_prod, dot_prod)); 37 | } 38 | 39 | std::ostream& operator<<(std::ostream& os, const Arc& arc) { 40 | os << "arc(" 41 | << "c=" << arc.center << " r=" << arc.radius << " b=" << arc.begin << " d=" << arc.delta 42 | << ")"; 43 | return os; 44 | } 45 | 46 | Poses Arc::trace(double step) const { 47 | VERIFY(step > 0); 48 | 49 | const size_t n = 1 + ceil(len() / step); 50 | const AngleVec2 angle_step(delta / n); 51 | 52 | Vec2 radius_vec = radius * begin; 53 | 54 | auto dir = AngleVec2::fromVector(ccw() > 0 ? begin.left() : begin.right()); 55 | 56 | Poses poses; 57 | poses.reserve(n); 58 | 59 | for (size_t i = 0; i < n; ++i) { 60 | poses.emplace_back(center + radius_vec, dir); 61 | radius_vec = angle_step.apply(radius_vec); 62 | dir = angle_step.apply(dir); 63 | } 64 | 65 | return poses; 66 | } 67 | 68 | } // namespace truck::geom 69 | -------------------------------------------------------------------------------- /packages/geom/src/bounding_box.cpp: -------------------------------------------------------------------------------- 1 | #include "geom/bounding_box.h" 2 | 3 | namespace truck::geom { 4 | 5 | BoundingBox& BoundingBox::extend(const Vec2& v) noexcept { 6 | min.x = std::min(min.x, v.x); 7 | min.y = std::min(min.y, v.y); 8 | max.x = std::max(max.x, v.x); 9 | max.y = std::max(max.y, v.y); 10 | return *this; 11 | } 12 | 13 | BoundingBox& BoundingBox::extend(double margin) noexcept { 14 | min.x -= margin; 15 | min.y -= margin; 16 | max.x += margin; 17 | max.y += margin; 18 | return *this; 19 | } 20 | 21 | } // namespace truck::geom 22 | -------------------------------------------------------------------------------- /packages/geom/src/circle.cpp: -------------------------------------------------------------------------------- 1 | #include "geom/circle.h" 2 | #include "geom/common.h" 3 | 4 | namespace truck::geom { 5 | 6 | bool equal(const Circle& a, const Circle& b, double eps) noexcept { 7 | return equal(a.radius, b.radius, eps) && equal(a.center, b.center, eps); 8 | } 9 | 10 | std::ostream& operator<<(std::ostream& out, const Circle& c) noexcept { 11 | return out << "Circle{" << c.center << ", " << c.radius << "}"; 12 | } 13 | 14 | } // namespace truck::geom 15 | -------------------------------------------------------------------------------- /packages/geom/src/distance.cpp: -------------------------------------------------------------------------------- 1 | #include "geom/distance.h" 2 | #include "geom/intersection.h" 3 | 4 | #include "common/math.h" 5 | 6 | namespace truck::geom { 7 | 8 | double distanceSq(const Vec2& a, const Vec2& b) noexcept { return (a - b).lenSq(); } 9 | 10 | double distance(const Vec2& a, const Vec2& b) noexcept { return (a - b).len(); } 11 | 12 | double distanceSq(const Vec2& p, const Segment& s) noexcept { 13 | const Vec2 ab = s.end - s.begin; 14 | const Vec2 ap = p - s.begin; 15 | const Vec2 bp = p - s.end; 16 | 17 | const double ab_len_sq = ab.lenSq(); 18 | const double ap_ab = dot(ap, ab); 19 | const double bp_ab = dot(bp, ab); 20 | 21 | if (ap_ab <= 0) { 22 | return ap.lenSq(); 23 | } 24 | 25 | if (bp_ab >= 0) { 26 | return bp.lenSq(); 27 | } 28 | 29 | return ap.lenSq() - squared(ap_ab) / ab_len_sq; 30 | } 31 | 32 | double denormalizedDistance(const Line& l, const Vec2& p) noexcept { 33 | return (dot(l.normal(), p) + l.c); 34 | } 35 | 36 | double denormalizedDistance(const Vec2& p, const Line& l) noexcept { 37 | return denormalizedDistance(l, p); 38 | } 39 | 40 | double distance(const Line& l, const Vec2& p) noexcept { 41 | return std::abs(denormalizedDistance(l, p) / l.normal().len()); 42 | } 43 | 44 | double distance(const Vec2& p, const Line& l) noexcept { return distance(l, p); } 45 | 46 | double distanceSq(const MotionState& a, const MotionState& b) noexcept { 47 | return distanceSq(a.pos, b.pos); 48 | } 49 | 50 | double distance(const MotionState& a, const MotionState& b) noexcept { 51 | return distance(a.pos, b.pos); 52 | } 53 | 54 | double distance(const Segment& a, const Segment& b) noexcept { 55 | if (intersect(a, b)) { 56 | return 0.0; 57 | } 58 | return std::min( 59 | {distance(a.begin, projection(a.begin, b)), 60 | distance(a.end, projection(a.end, b)), 61 | distance(b.begin, projection(b.begin, a)), 62 | distance(b.end, projection(b.end, a))}); 63 | } 64 | 65 | } // namespace truck::geom 66 | -------------------------------------------------------------------------------- /packages/geom/src/line.cpp: -------------------------------------------------------------------------------- 1 | #include "geom/line.h" 2 | 3 | #include "common/math.h" 4 | 5 | namespace truck::geom { 6 | 7 | bool equal(const Line& a, const Line& b, double eps) noexcept { 8 | return equal(a.normal() * b.c, b.normal() * a.c, eps); 9 | } 10 | 11 | std::ostream& operator<<(std::ostream& out, const Line& l) noexcept { 12 | return out << "Line(" << l.a << ", " << l.b << ", " << l.c << ")"; 13 | } 14 | 15 | } // namespace truck::geom 16 | -------------------------------------------------------------------------------- /packages/geom/src/motion_state.cpp: -------------------------------------------------------------------------------- 1 | #include "geom/motion_state.h" 2 | 3 | namespace truck::geom { 4 | Poses toPoses(const MotionStates& states) { 5 | Poses poses(states.size()); 6 | 7 | std::transform(states.begin(), states.end(), poses.begin(), [](const MotionState& state) { 8 | return state.pose(); 9 | }); 10 | 11 | return poses; 12 | } 13 | 14 | } // namespace truck::geom 15 | -------------------------------------------------------------------------------- /packages/geom/src/pose.cpp: -------------------------------------------------------------------------------- 1 | #include "geom/common.h" 2 | #include "geom/pose.h" 3 | 4 | #include "common/exception.h" 5 | 6 | #include 7 | 8 | namespace truck::geom { 9 | 10 | std::ostream& operator<<(std::ostream& out, const Pose& pose) noexcept { 11 | return out << std::fixed << std::setprecision(2) << "[" << pose.pos.x << ", " << pose.pos.y 12 | << ", " << Angle(pose.dir) << "]"; 13 | } 14 | 15 | Pose interpolate(const Pose& a, const Pose& b, double t) noexcept { 16 | VERIFY(0.0 <= t && t <= 1.0); 17 | 18 | const Vec2 pos = interpolate(a.pos, b.pos, t); 19 | const AngleVec2 dir = interpolate(a.dir, b.dir, t); 20 | return {pos, dir}; 21 | } 22 | 23 | } // namespace truck::geom 24 | -------------------------------------------------------------------------------- /packages/geom/src/segment.cpp: -------------------------------------------------------------------------------- 1 | #include "geom/segment.h" 2 | #include "geom/bezier.h" 3 | 4 | #include "common/math.h" 5 | 6 | namespace truck::geom { 7 | 8 | Poses Segment::trace(double step) const noexcept { return toPoses(bezier1(begin, end, step)); } 9 | 10 | Vec2 Segment::pos(double t) const noexcept { 11 | t = clamp(t, 0.0, 1.0); 12 | return begin * (1 - t) + end * t; 13 | } 14 | 15 | geom::Vec2 projection(const geom::Vec2& point, const geom::Segment& segment) noexcept { 16 | const geom::Vec2 dir(segment); 17 | 18 | const double t = dot(point - segment.begin, dir) / dir.lenSq(); 19 | return segment.pos(t); 20 | } 21 | 22 | } // namespace truck::geom 23 | -------------------------------------------------------------------------------- /packages/geom/src/transform.cpp: -------------------------------------------------------------------------------- 1 | #include "geom/transform.h" 2 | 3 | namespace truck::geom { 4 | 5 | Transform::Transform(Vec2 t, Angle a) : translation_(t), rotation_(a) {} 6 | 7 | Transform::Transform(Vec2 t, AngleVec2 r) : translation_(t), rotation_(r) {} 8 | 9 | namespace { 10 | 11 | geom::Vec2 toVector(const tf2::Vector3& v) { return {v.x(), v.y()}; } 12 | 13 | } // namespace 14 | 15 | geom::Angle toAngle(const tf2::Quaternion& q) { 16 | return geom::Angle::fromRadians(std::copysign(2 * std::acos(q.w()), q.z())); 17 | } 18 | 19 | Transform::Transform(const tf2::Transform& tf) : 20 | translation_(toVector(tf.getOrigin())), rotation_(toAngle(tf.getRotation())) {} 21 | 22 | Vec2 Transform::apply(Vec2 v) const { return translation_ + rotation_.apply(v); } 23 | 24 | Pose Transform::apply(Pose p) const { return Pose{apply(p.pos), rotation_.apply(p.dir)}; } 25 | 26 | Vec2 Transform::operator()(Vec2 v) const { return apply(v); } 27 | 28 | Pose Transform::operator()(Pose p) const { return apply(p); } 29 | 30 | Transform Transform::inv() const { 31 | auto r_inv = rotation_.inv(); 32 | return {r_inv.apply(-translation_), r_inv}; 33 | } 34 | 35 | std::ostream& operator<<(std::ostream& out, const Transform& transform) noexcept { 36 | return out << "TF[t=" << transform.t() << ", R=" << transform.r().angle() << "]"; 37 | } 38 | 39 | } // namespace truck::geom 40 | -------------------------------------------------------------------------------- /packages/geom/src/vector.cpp: -------------------------------------------------------------------------------- 1 | #include "geom/common.h" 2 | #include "geom/vector.h" 3 | 4 | #include "common/exception.h" 5 | 6 | #include 7 | 8 | namespace truck::geom { 9 | 10 | bool equal(const Vec2& a, const Vec2& b, double eps) noexcept { 11 | return equal(a.x, b.x, eps) && equal(a.y, b.y, eps); 12 | } 13 | 14 | geom::Angle angleBetween(const Vec2& a, const Vec2& b) noexcept { 15 | return geom::Angle::fromRadians(std::atan2(cross(a, b), dot(a, b))); 16 | } 17 | 18 | Vec2 interpolate(const Vec2& a, const Vec2& b, double t) noexcept { 19 | VERIFY(0 <= t && t <= 1); 20 | return a + t * (b - a); 21 | } 22 | 23 | std::ostream& operator<<(std::ostream& out, const Vec2& v) { 24 | return out << std::fixed << std::setprecision(3) << "[" << v.x << ", " << v.y << "]"; 25 | } 26 | 27 | } // namespace truck::geom 28 | -------------------------------------------------------------------------------- /packages/geom/src/vector3.cpp: -------------------------------------------------------------------------------- 1 | #include "geom/common.h" 2 | #include "geom/vector3.h" 3 | 4 | #include "common/exception.h" 5 | 6 | #include 7 | 8 | namespace truck::geom { 9 | 10 | bool equal(const Vec3& a, const Vec3& b, double eps) noexcept { 11 | return equal(a.x, b.x, eps) && equal(a.y, b.y, eps) && equal(a.z, b.z, eps); 12 | } 13 | 14 | Vec3 interpolate(const Vec3& a, const Vec3& b, double t) noexcept { 15 | VERIFY(0 <= t && t <= 1); 16 | return a + t * (b - a); 17 | } 18 | 19 | std::ostream& operator<<(std::ostream& out, const Vec3& v) { 20 | return out << std::fixed << std::setprecision(3) << "[" << v.x << ", " << v.y << ", " << v.z 21 | << "]"; 22 | } 23 | 24 | } // namespace truck::geom 25 | -------------------------------------------------------------------------------- /packages/geom/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest( 2 | ${PROJECT_NAME}_tests 3 | angle_tests.cpp 4 | angle_vector_tests.cpp 5 | arc_tests.cpp 6 | bounding_box_tests.cpp 7 | boost_tests.cpp 8 | distance_tests.cpp 9 | line_tests.cpp 10 | main.cpp 11 | polygon_tests.cpp 12 | ray_tests.cpp 13 | segment_tests.cpp 14 | uniform_stepper_tests.cpp 15 | vector_tests.cpp 16 | vector3_tests.cpp 17 | polyline_index_tests.cpp 18 | ) 19 | target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME}) 20 | 21 | set_tests_properties(${Tests} PROPERTIES TIMEOUT 1) 22 | 23 | target_include_directories( 24 | ${PROJECT_NAME}_tests 25 | PRIVATE "$" 26 | "$" 27 | "$" 28 | ) 29 | -------------------------------------------------------------------------------- /packages/geom/test/angle_tests.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "geom/angle.h" 4 | #include "geom/test/equal_assert.h" 5 | 6 | using namespace truck::geom; 7 | 8 | TEST(Angle, constructor) { 9 | const auto a = Angle::fromRadians(0); 10 | const auto b = Angle::fromDegrees(90); 11 | const auto c = Angle::fromVector(-1, 0); 12 | 13 | constexpr double eps = 1e-9; 14 | 15 | ASSERT_GEOM_EQUAL(a, PI0, eps); 16 | ASSERT_GEOM_EQUAL(b, PI_2, eps); 17 | ASSERT_GEOM_EQUAL(c, PI, eps); 18 | } 19 | 20 | TEST(Angle, coversion) { 21 | const auto a = PI_2; 22 | 23 | constexpr double eps = 1e-9; 24 | 25 | ASSERT_GEOM_EQUAL(a.radians(), M_PI / 2, eps); 26 | ASSERT_GEOM_EQUAL(a.degrees(), 90.0, eps); 27 | } 28 | 29 | TEST(Angle, literals) { 30 | using namespace truck::geom::literals; 31 | 32 | const auto a = 0_rad; 33 | const auto b = 90_deg; 34 | 35 | constexpr double eps = 1e-9; 36 | 37 | ASSERT_GEOM_EQUAL(a, PI0, eps); 38 | ASSERT_GEOM_EQUAL(b, PI_2, eps); 39 | } 40 | 41 | TEST(Angle, print) { 42 | std::stringstream ss; 43 | ss << Angle::fromDegrees(90); 44 | ASSERT_EQ(ss.str(), "90.00'"); 45 | } 46 | 47 | TEST(Angle, normalization) { 48 | const auto a = Angle::fromDegrees(90); 49 | const auto b = Angle::fromDegrees(360 + 45); 50 | const auto c = Angle::fromDegrees(-2 * 360 - 45); 51 | 52 | constexpr double eps = 1e-9; 53 | 54 | ASSERT_GEOM_EQUAL(a._0_2PI(), PI_2, eps); 55 | ASSERT_GEOM_EQUAL(b._0_2PI(), PI_4, eps); 56 | ASSERT_GEOM_EQUAL(c._0_2PI(), 7 * PI_4, eps); 57 | 58 | ASSERT_GEOM_EQUAL(a._mPI_PI(), PI_2, eps); 59 | ASSERT_GEOM_EQUAL(b._mPI_PI(), PI_4, eps); 60 | ASSERT_GEOM_EQUAL(c._mPI_PI(), -PI_4, eps); 61 | } 62 | -------------------------------------------------------------------------------- /packages/geom/test/angle_vector_tests.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "geom/angle_vector.h" 4 | #include "geom/test/equal_assert.h" 5 | 6 | using namespace truck::geom; 7 | 8 | TEST(AngleVec2, constructor) { 9 | const auto a = AngleVec2::fromVector(1, 1); 10 | const auto b = AngleVec2(PI_6); 11 | 12 | constexpr double eps = 1e-9; 13 | 14 | ASSERT_GEOM_EQUAL(a.angle(), PI_4, eps); 15 | ASSERT_GEOM_EQUAL(a.x(), std::sqrt(2) / 2, eps); 16 | ASSERT_GEOM_EQUAL(a.y(), std::sqrt(2) / 2, eps); 17 | 18 | ASSERT_GEOM_EQUAL(b.angle(), PI_6, eps); 19 | ASSERT_GEOM_EQUAL(b.x(), std::sqrt(3) / 2, eps); 20 | ASSERT_GEOM_EQUAL(b.y(), 0.5, eps); 21 | } 22 | 23 | TEST(AngleVec2, operation) { 24 | const auto a = AngleVec2(PI_2); 25 | const auto b = AngleVec2(PI_2); 26 | 27 | const auto sum = a + b; 28 | const auto diff = a - b; 29 | const auto inv = a.inv(); 30 | 31 | constexpr double eps = 1e-9; 32 | 33 | ASSERT_GEOM_EQUAL(sum.angle(), PI, eps); 34 | ASSERT_GEOM_EQUAL(sum.x(), -1., eps); 35 | ASSERT_GEOM_EQUAL(sum.y(), 0., eps); 36 | 37 | ASSERT_GEOM_EQUAL(diff.angle(), PI0, eps); 38 | ASSERT_GEOM_EQUAL(diff.x(), 1., eps); 39 | ASSERT_GEOM_EQUAL(diff.y(), 0., eps); 40 | 41 | ASSERT_GEOM_EQUAL(inv.angle(), -PI_2, eps); 42 | ASSERT_GEOM_EQUAL(inv.x(), 0., eps); 43 | ASSERT_GEOM_EQUAL(inv.y(), -1., eps); 44 | } 45 | 46 | TEST(AngleVec2, rotate) { 47 | const auto a = AngleVec2(Angle::zero()); 48 | 49 | const auto left = a.left(); 50 | const auto right = a.right(); 51 | const auto inv = AngleVec2(-PI).apply(a); 52 | 53 | constexpr double eps = 1e-9; 54 | 55 | ASSERT_GEOM_EQUAL(left.angle(), PI_2, eps); 56 | ASSERT_GEOM_EQUAL(left.x(), 0., eps); 57 | ASSERT_GEOM_EQUAL(left.y(), 1., eps); 58 | 59 | ASSERT_GEOM_EQUAL(right.angle(), -PI_2, eps); 60 | ASSERT_GEOM_EQUAL(right.x(), 0., eps); 61 | ASSERT_GEOM_EQUAL(right.y(), -1., eps); 62 | 63 | ASSERT_GEOM_EQUAL(inv.angle(), -PI, eps); 64 | ASSERT_GEOM_EQUAL(inv.x(), -1., eps); 65 | ASSERT_GEOM_EQUAL(inv.y(), 0., eps); 66 | } 67 | -------------------------------------------------------------------------------- /packages/geom/test/line_tests.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "geom/intersection.h" 4 | #include "geom/line.h" 5 | #include "geom/test/equal_assert.h" 6 | 7 | using namespace truck::geom; 8 | 9 | TEST(Line, make) { 10 | const Line l(1, 1, -2); 11 | 12 | auto l1 = Line::fromTwoPoints({0, 2}, {1, 1}); 13 | auto l2 = Line::fromPointAndNormal({-1, 3}, {1, 1}); 14 | auto l3 = Line::fromPointAndCollinear({4, -2}, {-1, 1}); 15 | 16 | ASSERT_GEOM_EQUAL(l, l1); 17 | ASSERT_GEOM_EQUAL(l, l2); 18 | ASSERT_GEOM_EQUAL(l, l3); 19 | } 20 | 21 | TEST(Line, intersect) { 22 | constexpr double eps = 1e-7; 23 | 24 | { 25 | auto l1 = Line::fromTwoPoints(Vec2(0, 0), Vec2(1, 1)); 26 | auto l2 = Line::fromTwoPoints(Vec2(0, 1), Vec2(1, 0)); 27 | 28 | ASSERT_GEOM_EQUAL(*intersect(l1, l2), Vec2(0.5, 0.5), eps); 29 | } 30 | 31 | { 32 | auto l1 = Line::fromTwoPoints(Vec2(0, 0), Vec2(0, 1)); 33 | auto l2 = Line::fromTwoPoints(Vec2(1, 0), Vec2(1, 1)); 34 | 35 | ASSERT_EQ(intersect(l1, l2), std::nullopt); 36 | } 37 | } 38 | -------------------------------------------------------------------------------- /packages/geom/test/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char* argv[]) { 4 | ::testing::InitGoogleTest(&argc, argv); 5 | return RUN_ALL_TESTS(); 6 | } 7 | -------------------------------------------------------------------------------- /packages/geom/test/polyline_index_tests.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "geom/polyline_index.h" 4 | #include "geom/bezier.h" 5 | #include "geom/test/equal_assert.h" 6 | 7 | using namespace truck::geom; 8 | 9 | TEST(PolylineIndex, it_works) { 10 | const PolylineIndex polyline = 11 | bezier3({0, 0}, {1, 1}, {2, 1}, {3, 0}, size_t(60)); 12 | 13 | double dist = 0; 14 | const double dist_inc = .1; 15 | 16 | AdvanceResult res = polyline.AdvanceFromBegin(); 17 | ASSERT_GEOM_EQUAL(Vec2{0, 0}, res.point.pos); 18 | 19 | while (!res.reached_end) { 20 | res = polyline.AdvanceFrom(res.poly_idx, dist_inc); 21 | dist += dist_inc; 22 | } 23 | 24 | ASSERT_GEOM_EQUAL(Vec2{3, 0}, res.point.pos); 25 | ASSERT_GEOM_EQUAL(dist, polyline.Length(), dist_inc); 26 | } 27 | 28 | TEST(PolylineIndex, even_steps) { 29 | const PolylineIndex polyline = 30 | bezier3({0, -5}, {10, -5}, {0, 5}, {-10, 5}, size_t(1023)); 31 | const double dist_inc = .2; 32 | 33 | Vec2 prev = polyline.AdvanceFromBegin().point.pos; 34 | 35 | for (auto it = polyline.AdvanceFromBegin(dist_inc); !it.reached_end; 36 | it = polyline.AdvanceFrom(it.poly_idx, dist_inc)) { 37 | ASSERT_GEOM_EQUAL(distance(prev, it.point.pos), dist_inc, 1e-3); 38 | prev = it.point.pos; 39 | } 40 | } 41 | 42 | TEST(PolylineIndex, edge_case) { 43 | const PolylineIndex polyline = 44 | bezier3({0, 0}, {1, 1}, {2, 1}, {3, 0}, .1); 45 | 46 | const auto end = polyline.AdvanceFromBegin(polyline.Length() + 1e3); 47 | ASSERT_TRUE(end.reached_end); 48 | 49 | const auto also_end = polyline.AdvanceFrom(end.poly_idx, 1e3); 50 | ASSERT_TRUE(also_end.reached_end); 51 | 52 | ASSERT_GEOM_EQUAL(end.point.pos, also_end.point.pos); 53 | } 54 | -------------------------------------------------------------------------------- /packages/geom/test/segment_tests.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "geom/intersection.h" 4 | #include "geom/segment.h" 5 | #include "geom/test/equal_assert.h" 6 | 7 | using namespace truck::geom; 8 | 9 | TEST(Segment, intersect) { 10 | { 11 | auto seg1 = Segment(Vec2(0, 0), Vec2(0, 1)); 12 | auto seg2 = Segment(Vec2(1, 0), Vec2(1, 1)); 13 | 14 | ASSERT_EQ(intersect(seg1, seg2), false); 15 | } 16 | 17 | { 18 | auto seg1 = Segment(Vec2(0, 0), Vec2(1, 0)); 19 | auto seg2 = Segment(Vec2(0, 1), Vec2(1, 1)); 20 | 21 | ASSERT_EQ(intersect(seg1, seg2), false); 22 | } 23 | 24 | { 25 | auto seg1 = Segment(Vec2(0, 0), Vec2(0, 1)); 26 | auto seg2 = Segment(Vec2(0, 1), Vec2(1, 1)); 27 | 28 | ASSERT_EQ(intersect(seg1, seg2), true); 29 | } 30 | 31 | { 32 | auto seg1 = Segment(Vec2(0, 0), Vec2(0, 1)); 33 | auto seg2 = Segment(Vec2(-1, 1), Vec2(1, 1)); 34 | 35 | ASSERT_EQ(intersect(seg1, seg2), true); 36 | } 37 | 38 | { 39 | auto seg1 = Segment(Vec2(0, 0), Vec2(0, 1)); 40 | auto seg2 = Segment(Vec2(-1, 2), Vec2(1, 1)); 41 | 42 | ASSERT_EQ(intersect(seg1, seg2), false); 43 | } 44 | 45 | { 46 | auto seg1 = Segment(Vec2(1, 0), Vec2(1, 2)); 47 | auto seg2 = Segment(Vec2(0, 1), Vec2(2, 1)); 48 | 49 | ASSERT_EQ(intersect(seg1, seg2), true); 50 | } 51 | 52 | { 53 | auto seg1 = Segment(Vec2(0, 0), Vec2(0, 1)); 54 | auto seg2 = Segment(Vec2(0, 0), Vec2(0, 1)); 55 | 56 | ASSERT_EQ(intersect(seg1, seg2), true); 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /packages/geom/test/vector3_tests.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "geom/test/equal_assert.h" 4 | #include "geom/vector3.h" 5 | 6 | using namespace truck::geom; 7 | 8 | TEST(Vector3, constructor) { 9 | const auto a = Vec3(); 10 | const auto b = Vec3{2, 5, 3}; 11 | const auto vec2 = Vec2{1, 3}; 12 | const auto c = Vec3(vec2, 4); 13 | const auto d = Vec3(vec2); 14 | 15 | constexpr double eps = 1e-9; 16 | 17 | ASSERT_GEOM_EQUAL(a.x, 0., eps); 18 | ASSERT_GEOM_EQUAL(a.y, 0., eps); 19 | ASSERT_GEOM_EQUAL(a.z, 0., eps); 20 | 21 | ASSERT_GEOM_EQUAL(b.x, 2., eps); 22 | ASSERT_GEOM_EQUAL(b.y, 5., eps); 23 | ASSERT_GEOM_EQUAL(b.z, 3., eps); 24 | 25 | ASSERT_GEOM_EQUAL(c.x, 1., eps); 26 | ASSERT_GEOM_EQUAL(c.y, 3., eps); 27 | ASSERT_GEOM_EQUAL(c.z, 4., eps); 28 | 29 | ASSERT_GEOM_EQUAL(d.x, 1., eps); 30 | ASSERT_GEOM_EQUAL(d.y, 3., eps); 31 | ASSERT_GEOM_EQUAL(d.z, 0., eps); 32 | } 33 | 34 | TEST(Vector3, len) { 35 | const Vec3 a = {1, 1, 1}; 36 | const Vec3 b = {-1, -1, -1}; 37 | const Vec3 c = {-1, 1, 0}; 38 | const Vec3 d = {2, 5, 7}; 39 | 40 | constexpr double eps = 1e-9; 41 | 42 | ASSERT_GEOM_EQUAL(a.len(), std::sqrt(3), eps); 43 | ASSERT_GEOM_EQUAL(b.len(), std::sqrt(3), eps); 44 | ASSERT_GEOM_EQUAL(c.len(), std::sqrt(2), eps); 45 | ASSERT_GEOM_EQUAL(d.len(), std::sqrt(78), eps); 46 | } 47 | -------------------------------------------------------------------------------- /packages/geom/test/vector_tests.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "geom/angle_vector.h" 4 | #include "geom/test/equal_assert.h" 5 | #include "geom/vector.h" 6 | 7 | using namespace truck::geom; 8 | 9 | TEST(Vector, constructor) { 10 | const auto a = Vec2{1, 3}; 11 | const auto b = Vec2::fromAngle(PI_2); 12 | const auto c = Vec2(AngleVec2(PI)); 13 | 14 | constexpr double eps = 1e-9; 15 | 16 | ASSERT_GEOM_EQUAL(a.x, 1., eps); 17 | ASSERT_GEOM_EQUAL(a.y, 3., eps); 18 | 19 | ASSERT_GEOM_EQUAL(b.x, 0., eps); 20 | ASSERT_GEOM_EQUAL(b.y, 1., eps); 21 | 22 | ASSERT_GEOM_EQUAL(c.x, -1., eps); 23 | ASSERT_GEOM_EQUAL(c.y, 0., eps); 24 | } 25 | 26 | TEST(Vector, len) { 27 | const Vec2 a = {1, 1}; 28 | ASSERT_GEOM_EQUAL(a.len(), std::sqrt(2), 1e-9); 29 | 30 | const Vec2 b = {-1, -1}; 31 | ASSERT_GEOM_EQUAL(b.len(), std::sqrt(2), 1e-9); 32 | 33 | const Vec2 c = {-1, 1}; 34 | ASSERT_GEOM_EQUAL(c.len(), std::sqrt(2), 1e-9); 35 | } 36 | 37 | TEST(Vector, rotate) { 38 | const Vec2 v = {1, 1}; 39 | 40 | const AngleVec2 a = PI_2; 41 | const AngleVec2 b = -PI_2; 42 | const AngleVec2 c = PI; 43 | const AngleVec2 d = PI * 4; 44 | 45 | constexpr double eps = 1e-9; 46 | 47 | ASSERT_GEOM_EQUAL(a.apply(v), Vec2{-1, 1}, eps); 48 | ASSERT_GEOM_EQUAL(b.apply(v), Vec2{1, -1}, eps); 49 | ASSERT_GEOM_EQUAL(c.apply(v), -v, eps); 50 | ASSERT_GEOM_EQUAL(d.apply(v), v, eps); 51 | } 52 | 53 | TEST(Vector, to_angle_vector) { 54 | const Vec2 a = {1, 1}; 55 | const Vec2 b = {-10, 0}; 56 | const Vec2 c = {0, -0.5}; 57 | 58 | ASSERT_GEOM_EQUAL(AngleVec2::fromVector(a), AngleVec2(PI_4), 1e-9); 59 | ASSERT_GEOM_EQUAL(AngleVec2::fromVector(b), AngleVec2(PI), 1e-9); 60 | ASSERT_GEOM_EQUAL(AngleVec2::fromVector(c), AngleVec2(-PI_2), 1e-9); 61 | } 62 | -------------------------------------------------------------------------------- /packages/hardware_node/hardware_node/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-laboratory/truck/332ff42e67d083e36dcf9d8070f3c9e8527ffc6e/packages/hardware_node/hardware_node/__init__.py -------------------------------------------------------------------------------- /packages/hardware_node/hardware_node/teensy.py: -------------------------------------------------------------------------------- 1 | import msgpacketizer as pack 2 | import numpy as np 3 | import serial 4 | 5 | 6 | class TeensyBridge: 7 | MSGPACK_SEND_INDEX = 1 8 | MSGPACK_RECV_INDEX = 2 9 | 10 | def __init__( 11 | self, 12 | logger, 13 | serial_port: str, 14 | serial_speed: int, 15 | steering_csv_path: str, 16 | servo_home_angles: dict, 17 | ): 18 | self._log = logger 19 | self._serial = serial.Serial(port=serial_port, baudrate=serial_speed) 20 | self._serial.reset_input_buffer() 21 | self._serial.reset_output_buffer() 22 | # Steering mapping for RIGHT wheel generated in CAD 23 | # Column 0 = wheel angle (radians), column 1 = servo angle (radians) 24 | # Wheel angle (+) = left turn (right wheel moves clockwise, looking from top) 25 | # Servo angle (-) = left turn (servo arm moves forward, looking from top) 26 | self._map = np.genfromtxt(steering_csv_path, delimiter=",", skip_header=True) 27 | self._servo_home_angles = servo_home_angles 28 | 29 | def push(self, left_wheel_angle: float, right_wheel_angle: float): 30 | self._log.debug( 31 | f"Input angles: " 32 | f"{left_wheel_angle / np.pi * 180:.1f} | " 33 | f"{right_wheel_angle / np.pi * 180:.1f}", 34 | ) 35 | left_servo_angle = np.interp( 36 | -left_wheel_angle, self._map[:, 0], self._map[:, 1] 37 | ) 38 | right_servo_angle = np.interp( 39 | right_wheel_angle, self._map[:, 0], self._map[:, 1] 40 | ) 41 | self._log.debug( 42 | f"Servo angles: " 43 | f"{left_servo_angle / np.pi * 180:.1f} | " 44 | f"{right_servo_angle / np.pi * 180:.1f}" 45 | ) 46 | left_servo_angle = self._servo_home_angles["left"] + left_servo_angle 47 | right_servo_angle = self._servo_home_angles["right"] - right_servo_angle 48 | data = (np.rad2deg(left_servo_angle), np.rad2deg(right_servo_angle)) 49 | self._log.debug(f"Servo angles + home: {data[0]:.1f} | {data[1]:.1f}") 50 | pack.send(self._serial, self.MSGPACK_SEND_INDEX, data) 51 | -------------------------------------------------------------------------------- /packages/hardware_node/launch/hardware_node.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | 3 | - arg: 4 | name: "model_config" 5 | default: "$(find-pkg-share model)/config/model.yaml" 6 | 7 | - arg: 8 | name: "steering_config" 9 | default: "$(find-pkg-share hardware_node)/resource/steering.csv" 10 | 11 | - node: 12 | pkg: "hardware_node" 13 | exec: "hardware_node" 14 | name: "hardware_node" 15 | output: "log" 16 | param: 17 | - { name: "model_config", value: "$(var model_config)" } 18 | - { name: "steering_config", value: "$(var steering_config)" } 19 | - { name: "odrive_timeout", value: 250 } # ms 20 | - { name: "status_report_rate", value: 1.0 } # Hz 21 | - { name: "telemetry_report_rate", value: 20.0 } # Hz 22 | -------------------------------------------------------------------------------- /packages/hardware_node/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hardware_node 4 | 0.0.1 5 | Node to control truck hardware (odrive + steering) 6 | AndBondStyle 7 | MIT 8 | 9 | rclpy 10 | truck_msgs 11 | model 12 | nav_msgs 13 | 14 | 15 | ament_python 16 | 17 | 18 | -------------------------------------------------------------------------------- /packages/hardware_node/readme.md: -------------------------------------------------------------------------------- 1 | # Harware Node 2 | 3 | ## Overview 4 | 5 | This package provides a ROS node to control robot movement on real (not simulated) hardware. More specifically, it communicates with ODrive (BLDC motor controller) and Teensy MCU (manages servo motors for steering). 6 | 7 | ## Paramethers 8 | 9 | - `model_config` — path to `model.yaml` file, used by `model::Model` class ([see here for details](../model/include/model/model.h)) 10 | - `steering_config` — path to `steering.csv` file ([see here for details](hardware_node/teensy.py)) 11 | - `odrive_timeout` (default: 250 ms) — [odrive watchog](https://docs.odriverobotics.com/v/0.5.4/getting-started.html#watchdog-timer) timeout (milliseconds). Adjust to be slightly higher than the interval between control messages (`/control/command` topic). 12 | - `status_report_rate` (default: 1 Hz) — rate of messages in `/hardware/status` topic. 13 | - `telemetry_report_rate` (default: 20 Hz) — rate of messages in `/hardware/telemetry` topic. 14 | - `odrive_axis` (default: "axis1") — name of odrive axis with connected motor. 15 | - `teensy_serial_port` (default: "/dev/ttyTHS0") — path to serial port to communicate with teensy. 16 | - `teensy_serial_speed` (default: 500000 baud) — teensy serial connection speed (baud rate). 17 | 18 | ## Topics 19 | 20 | ### Input: 21 | - `/control/command` — current velocity and steering target ([see here](../truck_msgs/msg/Control.msg)). 22 | - `/control/mode` — current mode of operation ([see here](../truck_msgs/msg/ControlMode.msg)). 23 | - `OFF` = motion disabled (odrive state = IDLE, motor coils off). 24 | - `REMOTE` = robot is controlled via remote (gamepad). 25 | - `AUTO` = robot is controlled programmatically. 26 | 27 | ### Output: 28 | - `/hardware/status` — global status of hardware ([see here](../truck_msgs/msg/HardwareStatus.msg)). In case of odrive failure, message will contain list of internal error codes. Published with a regular rate = `telemetry_report_rate`, also published immediatly after odrive error occurs. 29 | - `/hardware/telemetry` — odrive internal metrics, useful for diagnostic ([see here](../truck_msgs/msg/HardwareTelemetry.msg)). Published with a relatively high rate = `telemetry_report_rate`. 30 | -------------------------------------------------------------------------------- /packages/hardware_node/resource/hardware_node: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-laboratory/truck/332ff42e67d083e36dcf9d8070f3c9e8527ffc6e/packages/hardware_node/resource/hardware_node -------------------------------------------------------------------------------- /packages/hardware_node/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/hardware_node 3 | [install] 4 | install_scripts=$base/lib/hardware_node 5 | -------------------------------------------------------------------------------- /packages/hardware_node/setup.py: -------------------------------------------------------------------------------- 1 | from glob import glob 2 | 3 | from setuptools import setup 4 | 5 | package_name = "hardware_node" 6 | 7 | setup( 8 | name=package_name, 9 | version="0.0.1", 10 | packages=[package_name], 11 | data_files=[ 12 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 13 | (f"share/{package_name}", ["package.xml"]), 14 | (f"share/{package_name}/launch", glob("launch/*")), 15 | (f"share/{package_name}/resource", ["resource/steering.csv"]), 16 | ], 17 | install_requires=["setuptools"], 18 | zip_safe=True, 19 | entry_points={ 20 | "console_scripts": [ 21 | "hardware_node = hardware_node.node:main", 22 | ], 23 | }, 24 | ) 25 | -------------------------------------------------------------------------------- /packages/icp_odometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(icp_odometry) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package( 6 | Boost 7 | COMPONENTS thread 8 | filesystem 9 | system 10 | program_options 11 | date_time 12 | chrono 13 | timer 14 | serialization 15 | REQUIRED 16 | ) 17 | find_package(common REQUIRED) 18 | find_package(geom REQUIRED) 19 | find_package(libpointmatcher REQUIRED) 20 | find_package(sensor_msgs REQUIRED) 21 | find_package(truck_msgs REQUIRED) 22 | find_package(rclcpp REQUIRED) 23 | 24 | add_executable( 25 | ${PROJECT_NAME}_node src/main.cpp src/icp_odometry_node.cpp 26 | src/conversion.cpp 27 | ) 28 | 29 | ament_target_dependencies( 30 | ${PROJECT_NAME}_node 31 | common 32 | geom 33 | libpointmatcher 34 | rclcpp 35 | sensor_msgs 36 | Boost 37 | truck_msgs 38 | ) 39 | 40 | target_include_directories( 41 | ${PROJECT_NAME}_node 42 | PUBLIC "$" 43 | "$" 44 | "$" 45 | ) 46 | 47 | install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME}) 48 | 49 | install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) 50 | 51 | ament_package() 52 | -------------------------------------------------------------------------------- /packages/icp_odometry/config/icp.yaml: -------------------------------------------------------------------------------- 1 | readingDataPointsFilters: 2 | - RandomSamplingDataPointsFilter: 3 | prob: 0.5 4 | 5 | referenceDataPointsFilters: 6 | - SamplingSurfaceNormalDataPointsFilter: 7 | ratio: 0.5 8 | knn: 7 9 | 10 | matcher: 11 | KDTreeMatcher: 12 | knn: 1 13 | 14 | outlierFilters: 15 | - TrimmedDistOutlierFilter: 16 | ratio: 0.80 17 | 18 | errorMinimizer: 19 | PointToPlaneErrorMinimizer 20 | 21 | transformationCheckers: 22 | - CounterTransformationChecker: 23 | maxIterationCount: 50 24 | - DifferentialTransformationChecker: 25 | minDiffRotErr: 0.001 26 | minDiffTransErr: 0.01 27 | smoothLength: 4 28 | 29 | inspector: 30 | NullInspector 31 | 32 | logger: 33 | NullLogger 34 | -------------------------------------------------------------------------------- /packages/icp_odometry/include/icp_odometry/common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace truck::icp_odometry { 6 | 7 | using Matcher = PointMatcher; 8 | 9 | using DataPoints = Matcher::DataPoints; 10 | using Matrix = Matcher::Matrix; 11 | using TransformationParameters = Matcher::TransformationParameters; 12 | 13 | } // namespace truck::icp_odometry 14 | -------------------------------------------------------------------------------- /packages/icp_odometry/include/icp_odometry/conversion.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "icp_odometry/common.h" 7 | 8 | namespace truck::icp_odometry { 9 | 10 | DataPoints toDataPoints(const sensor_msgs::msg::LaserScan& scan); 11 | 12 | sensor_msgs::msg::PointCloud2 toPointCloud2( 13 | const std_msgs::msg::Header& header, const DataPoints& data_points); 14 | 15 | } // namespace truck::icp_odometry 16 | -------------------------------------------------------------------------------- /packages/icp_odometry/launch/icp_odometry.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: 3 | name: "simulation" 4 | default: "false" 5 | - arg: 6 | name: "qos" 7 | default: "0" 8 | - node: 9 | pkg: "icp_odometry" 10 | exec: "icp_odometry_node" 11 | name: "icp_odometry_node" 12 | output: "log" 13 | param: 14 | - { name: "use_sim_time", value: "$(var simulation)" } 15 | - { name: "qos", value: "$(var qos)" } 16 | - { name: "icp_config", value: "$(find-pkg-share icp_odometry)/config/icp.yaml" } 17 | -------------------------------------------------------------------------------- /packages/icp_odometry/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | icp_odometry 5 | 0.0.0 6 | icp_odometry 7 | root 8 | MIT 9 | 10 | ament_cmake 11 | 12 | ros2launch 13 | 14 | common 15 | geom 16 | sensor_msgs 17 | std_msgs 18 | rclcpp 19 | truck_msgs 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /packages/icp_odometry/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "icp_odometry/icp_odometry_node.h" 2 | #include "rclcpp/rclcpp.hpp" 3 | 4 | #include 5 | 6 | int main(int argc, char* argv[]) { 7 | rclcpp::init(argc, argv); 8 | rclcpp::spin(std::make_shared()); 9 | rclcpp::shutdown(); 10 | return 0; 11 | } 12 | -------------------------------------------------------------------------------- /packages/lidar_map/include/lidar_map/common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace truck::lidar_map { 7 | 8 | using Matcher = PointMatcher; 9 | using ICP = Matcher::ICP; 10 | using DataPoints = Matcher::DataPoints; 11 | 12 | /** 13 | * 4xn eigen matrix of 3D point cloud in homogeneous coordinates 14 | */ 15 | using Cloud = Eigen::Matrix4Xf; 16 | 17 | /** 18 | * 4xn eigen matrices of 3D point clouds in homogeneous coordinates 19 | */ 20 | using Clouds = std::vector; 21 | 22 | struct CloudWithAttributes { 23 | Cloud cloud; 24 | std::optional weights = std::nullopt; 25 | 26 | // normals_x, normals_y, normals_z - components of the normal vector that indicate the direction 27 | // perpendicular to the surface passing through the point. 28 | std::optional normals = std::nullopt; 29 | }; 30 | 31 | } // namespace truck::lidar_map 32 | -------------------------------------------------------------------------------- /packages/lidar_map/include/lidar_map/conversion.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "lidar_map/common.h" 4 | #include "geom/msg.h" 5 | #include "geom/pose.h" 6 | 7 | #include 8 | #include 9 | 10 | namespace truck::lidar_map { 11 | 12 | geom::Poses toPoses(const std::vector& odom_msgs); 13 | 14 | Cloud toCloud(const sensor_msgs::msg::PointCloud2& scan, const int intensity_threshold = 0); 15 | 16 | Clouds toClouds( 17 | const std::vector& scans, const int intensity_threshold = 0); 18 | 19 | namespace msg { 20 | 21 | sensor_msgs::msg::PointCloud2 toPointCloud2(const Cloud& cloud, std::string frame_id = "world"); 22 | 23 | sensor_msgs::msg::PointCloud2 toPointCloud2( 24 | const Cloud& cloud, const Eigen::VectorXf& weights, std::string frame_id = "world"); 25 | 26 | } // namespace msg 27 | 28 | } // namespace truck::lidar_map 29 | -------------------------------------------------------------------------------- /packages/lidar_map/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lidar_map 5 | 0.2.0 6 | lidar_map 7 | apmilko 8 | MIT 9 | 10 | ament_cmake 11 | 12 | geom 13 | common 14 | nav_msgs 15 | sensor_msgs 16 | visualization_msgs 17 | rosbag2_cpp 18 | libpointmatcher 19 | G2O 20 | PCL 21 | pcl_conversions 22 | ament_index_cpp 23 | Boost 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /packages/map/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(map) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(geom REQUIRED) 6 | find_package(common REQUIRED) 7 | find_package(nlohmann_json REQUIRED) 8 | 9 | add_library(${PROJECT_NAME} SHARED src/map.cpp) 10 | 11 | ament_target_dependencies(${PROJECT_NAME} geom common nlohmann_json) 12 | 13 | target_include_directories( 14 | ${PROJECT_NAME} 15 | PUBLIC "$" 16 | "$" 17 | ) 18 | 19 | ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) 20 | 21 | ament_export_dependencies(geom common nlohmann_json) 22 | 23 | install( 24 | TARGETS ${PROJECT_NAME} 25 | EXPORT ${PROJECT_NAME} 26 | ARCHIVE DESTINATION lib 27 | LIBRARY DESTINATION lib 28 | RUNTIME DESTINATION bin 29 | ) 30 | 31 | install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) 32 | 33 | install(DIRECTORY data DESTINATION share/${PROJECT_NAME}) 34 | 35 | ament_package() 36 | -------------------------------------------------------------------------------- /packages/map/include/map/map.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/complex_polygon.h" 4 | 5 | namespace truck::map { 6 | 7 | class Map { 8 | public: 9 | Map(geom::ComplexPolygons polygons); 10 | 11 | static Map fromGeoJson(const std::string& path); 12 | 13 | const geom::ComplexPolygons& polygons() const; 14 | 15 | private: 16 | geom::ComplexPolygons polygons_; 17 | }; 18 | 19 | } // namespace truck::map 20 | -------------------------------------------------------------------------------- /packages/map/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | map 5 | 0.0.0 6 | map 7 | apmilko 8 | MIT 9 | 10 | ament_cmake 11 | 12 | geom 13 | common 14 | nlohmann_json 15 | 16 | 17 | ament_cmake 18 | 19 | 20 | -------------------------------------------------------------------------------- /packages/map/src/map.cpp: -------------------------------------------------------------------------------- 1 | #include "map/map.h" 2 | 3 | #include "common/exception.h" 4 | 5 | #include 6 | 7 | #include 8 | 9 | namespace truck::map { 10 | 11 | Map Map::fromGeoJson(const std::string& path) { 12 | geom::ComplexPolygons polygons; 13 | const nlohmann::json geojson_features = nlohmann::json::parse(std::ifstream(path))["features"]; 14 | 15 | // iterate through every complex polygon 16 | for (const auto& elem : geojson_features) { 17 | // get list of outer (0 index) and inner polygons (1+ index) for a current complex polygon 18 | auto polys_list = elem["geometry"]["coordinates"] 19 | .get>>>(); 20 | 21 | int polys_cnt = 0; 22 | geom::Polygon outer; 23 | geom::Polygons inners; 24 | 25 | for (size_t i = 0; i < polys_list.size(); i++) { 26 | geom::Polygon simple_poly; 27 | for (const auto& point : polys_list[i]) { 28 | VERIFY(point.first >= 0 && point.second >= 0); 29 | simple_poly.push_back(geom::Vec2(point.first, point.second)); 30 | } 31 | 32 | if (polys_cnt == 0) { 33 | outer = std::move(simple_poly); 34 | } else { 35 | inners.push_back(std::move(simple_poly)); 36 | } 37 | 38 | polys_cnt++; 39 | } 40 | 41 | polygons.emplace_back(std::move(outer), std::move(inners)); 42 | } 43 | 44 | return {std::move(polygons)}; 45 | } 46 | 47 | Map::Map(geom::ComplexPolygons polygons) : polygons_(std::move(polygons)) {} 48 | 49 | const geom::ComplexPolygons& Map::polygons() const { return polygons_; } 50 | 51 | } // namespace truck::map 52 | -------------------------------------------------------------------------------- /packages/model/README.md: -------------------------------------------------------------------------------- 1 | # Model 2 | 3 | ## Overview 4 | [Ackeramnn model](../../doc/ackermann_vehicle.md) parameters and some simple calculations. 5 | 6 | ![This is an image](../../doc/svg/ackermann_vehicle.svg) 7 | 8 | # Model TF 9 | ## Overview 10 | Publishes static transformation of models (listed in yaml file). This is the simplest analogue of `static_transform_publisher` and `joint_state_publisher`, but for our own purpouses (use sdf model format). 11 | 12 | ## Parameters 13 | - `model_path` — path to yaml file with model config (including transform). 14 | - `period` — period of transforms publishing (milliseconds). 15 | 16 | ## Yaml format 17 | ``` 18 | - frame_id: "from" 19 | child_frame_id: "to" 20 | translation: {x: 0.0, y: 0.0, z: 0.0} 21 | rotation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} 22 | ``` 23 | 24 | ### Output: 25 | - `/tf` [[tf2_msgs/TFMessage]](http://docs.ros.org/en/api/tf2_msgs/html/msg/TFMessage.html) - transforms 26 | -------------------------------------------------------------------------------- /packages/model/include/model/params.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/vector.h" 4 | #include "geom/pose.h" 5 | #include "geom/angle.h" 6 | #include "common/math.h" 7 | #include "model/shape.h" 8 | 9 | #include "yaml-cpp/yaml.h" 10 | 11 | namespace truck::model { 12 | 13 | struct WheelBase { 14 | WheelBase(const YAML::Node& node); 15 | 16 | double width; 17 | double length; 18 | double base_to_rear; 19 | }; 20 | 21 | struct SteeringLimit { 22 | geom::Angle inner; 23 | geom::Angle outer; 24 | }; 25 | 26 | struct VehicleLimits { 27 | VehicleLimits(const YAML::Node& node); 28 | 29 | double max_abs_curvature; 30 | double steering_velocity; 31 | SteeringLimit steering; 32 | Limits velocity; 33 | double max_acceleration; 34 | double max_deceleration; 35 | }; 36 | 37 | struct ServoAngles { 38 | ServoAngles(const YAML::Node& node); 39 | 40 | geom::Angle left; 41 | geom::Angle right; 42 | }; 43 | 44 | struct Wheel { 45 | Wheel(const YAML::Node& node); 46 | 47 | double radius; 48 | double width; 49 | }; 50 | 51 | struct Lidar { 52 | Lidar(const YAML::Node& node); 53 | 54 | geom::Angle angle_min; 55 | geom::Angle angle_max; 56 | geom::Angle angle_increment; 57 | float range_min; 58 | float range_max; 59 | }; 60 | 61 | struct Params { 62 | Params(const YAML::Node& node); 63 | Params(const std::string& config_path); 64 | 65 | Shape shape; 66 | WheelBase wheel_base; 67 | Wheel wheel; 68 | Lidar lidar; 69 | VehicleLimits limits; 70 | double gear_ratio; 71 | ServoAngles servo_home_angles; 72 | }; 73 | 74 | } // namespace truck::model 75 | -------------------------------------------------------------------------------- /packages/model/include/model/shape.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/polygon.h" 4 | #include "geom/pose.h" 5 | 6 | #include "yaml-cpp/yaml.h" 7 | 8 | namespace truck::model { 9 | 10 | struct Shape { 11 | Shape(); 12 | Shape(const YAML::Node& node); 13 | 14 | double width; 15 | double length; 16 | double base_to_rear; 17 | int circles_count; 18 | 19 | double radius() const; 20 | std::vector getCircleDecomposition(const geom::Pose& ego_pose) const; 21 | geom::Polygon rearPoseToShapePolygon(const geom::Pose& rear_pose) const; 22 | geom::Polygon basePoseToShapePolygon(const geom::Pose& base_pose) const; 23 | }; 24 | 25 | } // namespace truck::model 26 | -------------------------------------------------------------------------------- /packages/model/launch/model_tf.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: 3 | name: "model_path" 4 | default: "$(find-pkg-share model)/config/model.yaml" 5 | - arg: 6 | name: "simulation" 7 | default: "false" 8 | - node: 9 | pkg: "model" 10 | exec: "model_tf_node" 11 | name: "model_tf_node" 12 | output: "log" 13 | param: 14 | - { name: "use_sim_time", value: "$(var simulation)" } 15 | - { name: "model_path", value: "$(var model_path)" } 16 | - { name: "period", value: 100 } 17 | -------------------------------------------------------------------------------- /packages/model/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | model 5 | 0.0.0 6 | model 7 | Simagin Denis 8 | MIT 9 | 10 | ament_cmake 11 | rclcpp 12 | common 13 | geom 14 | tf2_ros 15 | tf2_geometry_msgs 16 | 17 | 18 | ament_cmake 19 | 20 | 21 | -------------------------------------------------------------------------------- /packages/model/src/model_tf.cpp: -------------------------------------------------------------------------------- 1 | #include "model/model.h" 2 | 3 | #include 4 | 5 | namespace truck::model { 6 | 7 | using namespace std::literals; 8 | 9 | class ModelTfNode : public rclcpp::Node { 10 | public: 11 | ModelTfNode() : Node("ModelTfNode") { 12 | const auto model_path = this->declare_parameter("model_path", "model.yaml"); 13 | const auto static_qos = rclcpp::QoS(1).transient_local(); 14 | 15 | model_ = std::make_unique(model::load(this->get_logger(), model_path)); 16 | 17 | static_tf_signal_ = 18 | Node::create_publisher("/tf_static", static_qos); 19 | 20 | auto static_tf = model_->getTfStaticMsg(); 21 | 22 | RCLCPP_INFO( 23 | this->get_logger(), 24 | "Load %zu static transforms from %s", 25 | static_tf.transforms.size(), 26 | model_path.c_str()); 27 | 28 | const auto stamp = now(); 29 | for (auto&& tf : static_tf.transforms) { 30 | tf.header.stamp = stamp; 31 | } 32 | 33 | static_tf_signal_->publish(static_tf); 34 | } 35 | 36 | private: 37 | std::unique_ptr model_ = nullptr; 38 | 39 | // output 40 | rclcpp::Publisher::SharedPtr static_tf_signal_ = nullptr; 41 | }; 42 | 43 | } // namespace truck::model 44 | 45 | int main(int argc, char* argv[]) { 46 | rclcpp::init(argc, argv); 47 | rclcpp::spin(std::make_shared()); 48 | rclcpp::shutdown(); 49 | return 0; 50 | } 51 | -------------------------------------------------------------------------------- /packages/model/src/shape.cpp: -------------------------------------------------------------------------------- 1 | #include "model/shape.h" 2 | 3 | #include "geom/vector.h" 4 | 5 | #include 6 | 7 | namespace truck::model { 8 | 9 | Shape::Shape() = default; 10 | 11 | Shape::Shape(const YAML::Node& node) : 12 | width(node["width"].as()), 13 | length(node["length"].as()), 14 | base_to_rear(node["base_to_rear"].as()), 15 | circles_count(node["circles_count"].as()) { 16 | BOOST_VERIFY(width > 0); 17 | BOOST_VERIFY(length > 0); 18 | BOOST_VERIFY(base_to_rear > 0); 19 | BOOST_VERIFY(length > base_to_rear); 20 | BOOST_VERIFY(circles_count * 2 * radius() > length); 21 | } 22 | 23 | double Shape::radius() const { return width / 2; } 24 | 25 | std::vector Shape::getCircleDecomposition(const geom::Pose& ego_pose) const { 26 | std::vector points; 27 | const double pos_first = -base_to_rear + radius(); 28 | const double pos_step = (length - 2 * radius()) / (circles_count - 1); 29 | 30 | for (int i = 0; i < circles_count; i++) { 31 | const double offset = pos_first + (i * pos_step); 32 | points.push_back(ego_pose.pos + offset * ego_pose.dir); 33 | } 34 | 35 | return points; 36 | } 37 | 38 | geom::Polygon Shape::rearPoseToShapePolygon(const geom::Pose& rear_pose) const { 39 | const auto x = rear_pose.pos.x; 40 | const auto y = rear_pose.pos.y; 41 | const auto yaw = rear_pose.dir.vec(); 42 | 43 | const auto dir = length * yaw; 44 | const auto norm = width / 2 * yaw; 45 | const geom::Vec2 a(x - norm.y, y + norm.x); 46 | const geom::Vec2 b(x + norm.y, y - norm.x); 47 | return {a, a + dir, b + dir, b}; 48 | } 49 | 50 | geom::Polygon Shape::basePoseToShapePolygon(const geom::Pose& base_pose) const { 51 | const geom::Pose rear_pose = {base_pose.pos - base_to_rear * base_pose.dir, base_pose.dir}; 52 | return rearPoseToShapePolygon(rear_pose); 53 | } 54 | 55 | } // namespace truck::model 56 | -------------------------------------------------------------------------------- /packages/motion/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(motion) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(common REQUIRED) 6 | find_package(geom REQUIRED) 7 | find_package(model REQUIRED) 8 | find_package(truck_msgs REQUIRED) 9 | 10 | add_library(${PROJECT_NAME} SHARED src/trajectory.cpp src/primitive.cpp) 11 | 12 | ament_target_dependencies(${PROJECT_NAME} common geom model truck_msgs) 13 | 14 | target_include_directories( 15 | ${PROJECT_NAME} 16 | PUBLIC "$" 17 | "$" 18 | ) 19 | 20 | ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) 21 | ament_export_dependencies(common model truck_msgs) 22 | 23 | install( 24 | TARGETS ${PROJECT_NAME} 25 | EXPORT ${PROJECT_NAME}Targets 26 | ARCHIVE DESTINATION lib 27 | LIBRARY DESTINATION lib 28 | RUNTIME DESTINATION bin 29 | ) 30 | 31 | install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) 32 | 33 | ament_package() 34 | -------------------------------------------------------------------------------- /packages/motion/include/motion/primitive.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/pose.h" 4 | 5 | namespace truck::geom { 6 | 7 | Poses findMotion(const Pose& from, const Vec2& to, double gamma, double step); 8 | 9 | Poses findMotion(const Pose& from, const Pose& to, double gamma, double step); 10 | 11 | } // namespace truck::geom 12 | -------------------------------------------------------------------------------- /packages/motion/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | motion 5 | 0.0.0 6 | motion 7 | Simagin Denis 8 | MIT 9 | 10 | ament_cmake 11 | 12 | common 13 | geom 14 | geometry_msgs 15 | model 16 | truck_msgs 17 | 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /packages/motion/src/primitive.cpp: -------------------------------------------------------------------------------- 1 | #include "motion/primitive.h" 2 | #include "geom/bezier.h" 3 | 4 | namespace truck::geom { 5 | 6 | Poses findMotion(const Pose& from, const Vec2& to, double gamma, double step) { 7 | const Vec2 middle = from.pos + from.dir * gamma; 8 | return toPoses(bezier2(from.pos, middle, to, step)); 9 | } 10 | 11 | Poses findMotion(const Pose& from, const Pose& to, double gamma, double step) { 12 | const Vec2 from_ref = from.pos + from.dir * gamma; 13 | const Vec2 to_ref = to.pos - to.dir * gamma; 14 | return toPoses(bezier3(from.pos, from_ref, to_ref, to.pos, step)); 15 | } 16 | 17 | } // namespace truck::geom 18 | -------------------------------------------------------------------------------- /packages/navigation/include/navigation/graph_builder.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/complex_polygon.h" 4 | 5 | namespace truck::navigation::graph { 6 | 7 | struct GraphParams { 8 | enum class Mode : uint8_t { kNearest = 0, searchRadius = 1 } mode; 9 | size_t k_nearest = 6; 10 | double search_radius = 2; 11 | double safe_zone_radius = 2.0; 12 | }; 13 | 14 | using NodeId = size_t; 15 | using EdgeId = size_t; 16 | 17 | struct Node { 18 | NodeId id; 19 | geom::Vec2 point; 20 | std::vector edges; 21 | }; 22 | 23 | struct Edge { 24 | NodeId from, to; 25 | double weight; 26 | }; 27 | 28 | using Nodes = std::vector; 29 | using Edges = std::vector; 30 | 31 | struct Graph { 32 | Nodes nodes; 33 | Edges edges; 34 | }; 35 | 36 | class GraphBuilder { 37 | public: 38 | GraphBuilder(const GraphParams& params); 39 | 40 | Graph build(const std::vector& mesh, const geom::ComplexPolygons& polygons) const; 41 | 42 | private: 43 | GraphParams params_; 44 | }; 45 | 46 | } // namespace truck::navigation::graph 47 | -------------------------------------------------------------------------------- /packages/navigation/include/navigation/mesh_builder.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/vector.h" 4 | #include "geom/segment.h" 5 | #include "geom/polyline.h" 6 | #include "geom/complex_polygon.h" 7 | 8 | namespace truck::navigation::mesh { 9 | 10 | struct MeshParams { 11 | double dist; 12 | double offset; 13 | 14 | struct RadialFilter { 15 | bool enabled; 16 | double search_radius; 17 | } filter; 18 | }; 19 | 20 | struct MeshBuild { 21 | geom::Segments skeleton; 22 | std::vector level_lines; 23 | std::vector mesh; 24 | }; 25 | 26 | class MeshBuilder { 27 | public: 28 | MeshBuilder(const MeshParams& params); 29 | 30 | MeshBuild build(const geom::ComplexPolygons& polygons) const; 31 | 32 | private: 33 | void buildSkeleton(MeshBuild& mesh_build, const geom::ComplexPolygon& polygon) const; 34 | void buildLevelLines( 35 | MeshBuild& mesh_build, const geom::ComplexPolygon& polygon, double offset) const; 36 | void buildMesh(MeshBuild& mesh_build, double dist) const; 37 | 38 | void applyMeshFilter(MeshBuild& mesh_build, double search_radius) const; 39 | 40 | MeshParams params_; 41 | }; 42 | 43 | } // namespace truck::navigation::mesh 44 | -------------------------------------------------------------------------------- /packages/navigation/include/navigation/search.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "navigation/graph_builder.h" 4 | 5 | #include "geom/polyline.h" 6 | #include "common/exception.h" 7 | 8 | namespace truck::navigation::search { 9 | 10 | struct Path { 11 | double length; 12 | std::vector trace; 13 | }; 14 | 15 | geom::Polyline toPolyline(const graph::Graph& graph, const Path& path); 16 | 17 | Path findShortestPath(const graph::Graph& graph, graph::NodeId from, graph::NodeId to); 18 | 19 | } // namespace truck::navigation::search 20 | -------------------------------------------------------------------------------- /packages/navigation/include/navigation/viewer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/polyline.h" 4 | #include "navigation/mesh_builder.h" 5 | #include "navigation/graph_builder.h" 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace truck::navigation::viewer { 13 | 14 | struct ViewerParams { 15 | double res = 50; 16 | std::string path = ""; 17 | 18 | struct ColorRGB { 19 | std::vector background = {0, 0, 0}; 20 | 21 | struct Polygon { 22 | std::vector inner = {227, 227, 227}; 23 | std::vector outer = {252, 252, 252}; 24 | } polygon; 25 | 26 | struct MeshBuild { 27 | std::vector level_lines = {255, 0, 0}; 28 | std::vector skeleton = {0, 200, 0}; 29 | std::vector mesh = {0, 0, 255}; 30 | } mesh_build; 31 | 32 | struct Graph { 33 | std::vector edges = {50, 50, 50}; 34 | std::vector nodes = {0, 0, 255}; 35 | } graph; 36 | 37 | std::vector path = {0, 200, 0}; 38 | } color_rgb; 39 | 40 | struct Thickness { 41 | struct MeshBuild { 42 | double level_lines = 1.0; 43 | double skeleton = 2.0; 44 | double mesh = 5.0; 45 | } mesh_build; 46 | 47 | struct Graph { 48 | double edges = 1.0; 49 | double nodes = 5.0; 50 | } graph; 51 | 52 | double path = 20.0; 53 | } thickness; 54 | }; 55 | 56 | class Viewer { 57 | public: 58 | Viewer(const ViewerParams& params, const geom::ComplexPolygon& polygon); 59 | 60 | void addPolygon(const geom::ComplexPolygon& polygon); 61 | void addMeshBuild(const mesh::MeshBuild& mesh_build); 62 | void addGraph(const graph::Graph& graph); 63 | void addPath(const geom::Polyline& path); 64 | 65 | void draw(); 66 | 67 | private: 68 | cv::Mat frame_; 69 | cv::Rect bbox_; 70 | geom::Vec2 bbox_origin_; 71 | 72 | ViewerParams params_; 73 | }; 74 | 75 | } // namespace truck::navigation::viewer 76 | -------------------------------------------------------------------------------- /packages/navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | navigation 5 | 0.0.0 6 | navigation 7 | apmilko 8 | MIT 9 | 10 | ament_cmake 11 | 12 | common 13 | geom 14 | map 15 | CGAL 16 | Boost 17 | OpenCV 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /packages/occupancy_grid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(occupancy_grid) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(common REQUIRED) 6 | find_package(cv_bridge REQUIRED) 7 | find_package(geom REQUIRED) 8 | find_package(image_geometry REQUIRED) 9 | find_package(image_transport REQUIRED) 10 | find_package(nav_msgs REQUIRED) 11 | find_package(OpenCV REQUIRED) 12 | find_package(rclcpp REQUIRED) 13 | find_package(sensor_msgs REQUIRED) 14 | find_package(std_msgs REQUIRED) 15 | find_package(tf2_geometry_msgs REQUIRED) 16 | 17 | add_executable(occupancy_grid_node src/main.cpp src/occupancy_grid_node.cpp) 18 | 19 | ament_target_dependencies( 20 | occupancy_grid_node 21 | common 22 | cv_bridge 23 | geom 24 | image_geometry 25 | image_transport 26 | nav_msgs 27 | rclcpp 28 | sensor_msgs 29 | std_msgs 30 | tf2_geometry_msgs 31 | ) 32 | 33 | target_include_directories( 34 | ${PROJECT_NAME}_node 35 | PUBLIC "$" 36 | "$" 37 | "$" 38 | ) 39 | 40 | install(TARGETS occupancy_grid_node DESTINATION lib/${PROJECT_NAME}) 41 | 42 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 43 | 44 | ament_package() 45 | -------------------------------------------------------------------------------- /packages/occupancy_grid/launch/occupancy_grid.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: 3 | name: "simulation" 4 | default: "false" 5 | - arg: 6 | name: "qos" 7 | default: "0" 8 | - arg: 9 | name: "enable_lidar_grid" 10 | default: "true" 11 | - arg: 12 | name: "enable_camera_grid" 13 | default: "false" 14 | - arg: 15 | name: "enable_camera_cloud" 16 | default: "false" 17 | - node: 18 | pkg: "occupancy_grid" 19 | exec: "occupancy_grid_node" 20 | name: "occupancy_grid_node" 21 | output: "log" 22 | param: 23 | - { name: "use_sim_time", value: "$(var simulation)" } 24 | - { name: "qos", value: "$(var qos)" } 25 | - { name: "resolution", value: 0.05 } 26 | - { name: "radius", value: 6.0 } 27 | - { name: "enable_camera_cloud", value: "$(var enable_camera_cloud)" } 28 | - { name: "enable_lidar_grid", value: "$(var enable_lidar_grid)" } 29 | - { name: "enable_camera_grid", value: "$(var enable_camera_grid)" } 30 | - { name: "camera_view_distance", value: 2.0 } 31 | - { name: "camera_view_hmax", value: 0.15 } 32 | - { name: "camera_view_hmin", value: -0.165 } 33 | -------------------------------------------------------------------------------- /packages/occupancy_grid/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | occupancy_grid 5 | 0.0.0 6 | occupancy_grid 7 | Siamgin Denis 8 | MIT 9 | 10 | ament_cmake 11 | 12 | ros2launch 13 | 14 | common 15 | cv_bridge 16 | geom 17 | image_geometry 18 | image_transport 19 | nav_msgs 20 | OpenCV 21 | rclcpp 22 | sensor_msgs 23 | std_msgs 24 | tf2_geometry_msgs 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /packages/occupancy_grid/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "occupancy_grid/occupancy_grid_node.h" 2 | #include "rclcpp/rclcpp.hpp" 3 | 4 | #include 5 | 6 | int main(int argc, char* argv[]) { 7 | rclcpp::init(argc, argv); 8 | rclcpp::spin(std::make_shared()); 9 | rclcpp::shutdown(); 10 | return 0; 11 | } 12 | -------------------------------------------------------------------------------- /packages/perf/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(perf) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(rclcpp REQUIRED) 6 | find_package(truck_msgs REQUIRED) 7 | 8 | add_executable(perf_stat_node src/node.cpp src/stat.cpp src/main.cpp) 9 | 10 | ament_target_dependencies(perf_stat_node rclcpp truck_msgs) 11 | 12 | target_include_directories( 13 | perf_stat_node 14 | PUBLIC "$" 15 | "$" 16 | "$" 17 | ) 18 | 19 | install(TARGETS perf_stat_node DESTINATION lib/${PROJECT_NAME}) 20 | 21 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 22 | 23 | ament_package() 24 | -------------------------------------------------------------------------------- /packages/perf/include/perf/node.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "perf/stat.h" 4 | 5 | #include "truck_msgs/msg/perf_stat.hpp" 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace truck::perf { 13 | 14 | class PerfStatNode : public rclcpp::Node { 15 | public: 16 | PerfStatNode(); 17 | 18 | void OnTimer(); 19 | 20 | private: 21 | std::optional stat_ = {}; 22 | rclcpp::TimerBase::SharedPtr timer_ = nullptr; 23 | rclcpp::Publisher::SharedPtr stat_signal_ = nullptr; 24 | }; 25 | 26 | } // namespace truck::perf 27 | -------------------------------------------------------------------------------- /packages/perf/include/perf/stat.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace truck::perf { 7 | 8 | struct CpuStat { 9 | int16_t id = -1; // -1 means total 10 | 11 | uint64_t user = 0; 12 | uint64_t nice = 0; 13 | uint64_t system; 14 | uint64_t idle = 0; 15 | uint64_t iowait = 0; 16 | uint64_t irq = 0; 17 | uint64_t softirq = 0; 18 | uint64_t steal = 0; 19 | uint64_t guest = 0; 20 | uint64_t guest_nice = 0; 21 | 22 | uint64_t total() const; 23 | uint64_t used() const; 24 | 25 | float usageRatio() const; 26 | uint16_t usagePercent() const; 27 | 28 | CpuStat operator-(const CpuStat& other) const; 29 | 30 | static std::vector read(); 31 | }; 32 | 33 | struct MemStat { 34 | uint64_t total = 0; 35 | uint64_t free = 0; 36 | uint64_t available = 0; 37 | 38 | uint64_t used() const; 39 | 40 | float usageRatio() const; 41 | uint16_t usagePercent() const; 42 | 43 | static MemStat read(); 44 | }; 45 | 46 | struct Stat { 47 | std::vector cpu; 48 | MemStat mem; 49 | 50 | static Stat read(); 51 | }; 52 | 53 | } // namespace truck::perf 54 | -------------------------------------------------------------------------------- /packages/perf/launch/perf_stat.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - node: 3 | pkg: "perf" 4 | exec: "perf_stat_node" 5 | name: "perf_stat_node" 6 | output: "log" 7 | -------------------------------------------------------------------------------- /packages/perf/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | perf 5 | 0.0.0 6 | Perf stats 7 | Simagin Denis 8 | MIT 9 | 10 | ament_cmake 11 | ros2launch 12 | 13 | rclcpp 14 | truck_msgs 15 | 16 | 17 | ament_cmake 18 | 19 | 20 | -------------------------------------------------------------------------------- /packages/perf/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "perf/node.h" 2 | 3 | #include 4 | 5 | #include 6 | 7 | int main(int argc, char* argv[]) { 8 | rclcpp::init(argc, argv); 9 | rclcpp::spin(std::make_shared()); 10 | rclcpp::shutdown(); 11 | return 0; 12 | } 13 | -------------------------------------------------------------------------------- /packages/perf/src/node.cpp: -------------------------------------------------------------------------------- 1 | #include "perf/node.h" 2 | #include "perf/stat.h" 3 | 4 | #include "truck_msgs/msg/cpu_stat.hpp" 5 | #include "truck_msgs/msg/mem_stat.hpp" 6 | 7 | #include 8 | 9 | namespace truck::perf { 10 | 11 | PerfStatNode::PerfStatNode() : rclcpp::Node("PerfStat") { 12 | constexpr auto period = std::chrono::duration(0.2); 13 | stat_signal_ = this->create_publisher("/perf/stat", 10); 14 | timer_ = this->create_wall_timer(period, std::bind(&PerfStatNode::OnTimer, this)); 15 | RCLCPP_INFO(this->get_logger(), "PerfStatNode started (%.2f Hz)", 1.0 / period.count()); 16 | } 17 | 18 | namespace { 19 | 20 | truck_msgs::msg::MemStat toMsg(const MemStat& stat) { 21 | auto msg = truck_msgs::msg::MemStat(); 22 | msg.total = stat.total; 23 | msg.free = stat.free; 24 | msg.usage = stat.usageRatio(); 25 | return msg; 26 | } 27 | 28 | truck_msgs::msg::CpuStat toMsg(const CpuStat& stat) { 29 | auto msg = truck_msgs::msg::CpuStat(); 30 | msg.id = stat.id; 31 | msg.usage = stat.usageRatio(); 32 | return msg; 33 | } 34 | 35 | } // namespace 36 | 37 | void PerfStatNode::OnTimer() { 38 | auto stat = Stat::read(); 39 | 40 | if (stat_) { 41 | auto msg = truck_msgs::msg::PerfStat(); 42 | msg.header.stamp = this->now(); 43 | msg.header.frame_id = "base"; 44 | 45 | for (size_t i = 0; i < stat.cpu.size(); ++i) { 46 | msg.cpu.push_back(toMsg(stat.cpu[i] - stat_->cpu[i])); 47 | } 48 | 49 | msg.mem = toMsg(stat.mem); 50 | stat_signal_->publish(msg); 51 | } 52 | 53 | stat_ = std::move(stat); 54 | } 55 | 56 | } // namespace truck::perf 57 | -------------------------------------------------------------------------------- /packages/planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(planner) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(rclcpp REQUIRED) 6 | find_package(geom REQUIRED) 7 | find_package(model REQUIRED) 8 | find_package(collision REQUIRED) 9 | find_package(nav_msgs REQUIRED) 10 | find_package(visualization_msgs REQUIRED) 11 | find_package(tf2 REQUIRED) 12 | find_package(tf2_ros REQUIRED) 13 | find_package(Boost REQUIRED) 14 | 15 | add_executable( 16 | ${PROJECT_NAME}_node src/main.cpp src/planner_node.cpp src/search.cpp 17 | ) 18 | 19 | ament_target_dependencies( 20 | ${PROJECT_NAME}_node 21 | rclcpp 22 | geom 23 | model 24 | collision 25 | nav_msgs 26 | visualization_msgs 27 | tf2 28 | tf2_ros 29 | Boost 30 | ) 31 | 32 | install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME}) 33 | 34 | target_include_directories( 35 | ${PROJECT_NAME}_node 36 | PUBLIC "$" 37 | "$" 38 | "$" 39 | ) 40 | 41 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 42 | 43 | ament_package() 44 | -------------------------------------------------------------------------------- /packages/planner/launch/planner.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: 3 | name: "model_config" 4 | default: "$(find-pkg-share model)/config/model.yaml" 5 | - node: 6 | pkg: "planner" 7 | exec: "planner_node" 8 | name: "planner_node" 9 | output: "log" 10 | param: 11 | - { name: "model_config", value: "$(var model_config)" } 12 | 13 | - { name: "grid/nodes/width", value: 40 } 14 | - { name: "grid/nodes/height", value: 40 } 15 | - { name: "grid/resolution", value: 0.2 } 16 | - { name: "grid/finish_area_radius", value: 0.5 } 17 | - { name: "grid/min_obstacle_distance", value: 0.5 } 18 | 19 | - { name: "node/z-lev", value: 0.05 } 20 | - { name: "node/scale", value: 0.1 } 21 | - { name: "node/base/color_rgba", value: [1.0, 0.2, 0.2, 0.2] } 22 | - { name: "node/ego/color_rgba", value: [1.0, 0.8, 0.8, 0.8] } 23 | - { name: "node/finish/color_rgba", value: [1.0, 1.0, 1.0, 0.0] } 24 | - { name: "node/finish_area/color_rgba", value: [0.3, 1.0, 1.0, 0.0] } 25 | - { name: "node/collision/color_rgba", value: [0.8, 1.0, 0.0, 0.0] } 26 | -------------------------------------------------------------------------------- /packages/planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | planner 5 | 0.0.0 6 | planner 7 | root 8 | MIT 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | geom 14 | model 15 | collision 16 | nav_msgs 17 | visualization_msgs 18 | tf2 19 | tf2_ros 20 | Boost 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /packages/planner/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "planner/planner_node.h" 2 | 3 | #include 4 | 5 | int main(int argc, char* argv[]) { 6 | rclcpp::init(argc, argv); 7 | rclcpp::spin(std::make_shared()); 8 | rclcpp::shutdown(); 9 | return 0; 10 | } 11 | -------------------------------------------------------------------------------- /packages/pure_pursuit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(pure_pursuit) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(geom REQUIRED) 6 | find_package(geometry_msgs REQUIRED) 7 | find_package(model REQUIRED) 8 | find_package(motion REQUIRED) 9 | find_package(rclcpp REQUIRED) 10 | find_package(truck_msgs REQUIRED) 11 | 12 | add_executable( 13 | ${PROJECT_NAME}_node src/pure_pursuit.cpp src/pure_pursuit_node.cpp 14 | src/main.cpp 15 | ) 16 | 17 | ament_target_dependencies( 18 | ${PROJECT_NAME}_node 19 | geom 20 | geometry_msgs 21 | model 22 | motion 23 | nav_msgs 24 | rclcpp 25 | truck_msgs 26 | visualization_msgs 27 | ) 28 | 29 | install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME}) 30 | 31 | target_include_directories( 32 | ${PROJECT_NAME}_node 33 | PUBLIC "$" 34 | "$" 35 | "$" 36 | ) 37 | 38 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 39 | 40 | ament_package() 41 | -------------------------------------------------------------------------------- /packages/pure_pursuit/include/pure_pursuit/pure_pursuit.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "common/math.h" 4 | #include "common/result.h" 5 | #include "geom/localization.h" 6 | #include "model/model.h" 7 | #include "motion/trajectory.h" 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace truck::pure_pursuit { 18 | 19 | enum class Error : uint8_t { 20 | kUnknown = 0, 21 | kNoProjection = 1, 22 | kUnreachableProjection = 2, 23 | kImpossibleBuildArc = 3, 24 | }; 25 | 26 | std::string_view toString(Error e); 27 | 28 | struct Command { 29 | double curvature = 0; 30 | double velocity = 0; 31 | double acceleration = 0; 32 | 33 | std::optional target = std::nullopt; 34 | 35 | static Command stop() { return Command{}; } 36 | }; 37 | 38 | using Result = common::Result; 39 | 40 | using namespace std::chrono_literals; 41 | 42 | struct Parameters { 43 | std::chrono::duration period = 0.1s; 44 | Limits radius = {0.15, 0.5}; 45 | double velocity_factor = 0.2; 46 | double tolerance = 0.2; 47 | double max_distance = 0.30; 48 | }; 49 | 50 | class PurePursuit { 51 | public: 52 | PurePursuit(const Parameters& params, const model::Model& model) : 53 | params_{params}, model_(model) {} 54 | 55 | Result operator()(const geom::Localization& localization, const motion::Trajectory& trajectory); 56 | 57 | Result command(const geom::Localization& localization, const motion::Trajectory& trajectory) { 58 | return (*this)(localization, trajectory); 59 | } 60 | 61 | private: 62 | double getRadius(double velocity) const; 63 | 64 | const Parameters params_; 65 | const model::Model model_; 66 | }; 67 | 68 | } // namespace truck::pure_pursuit 69 | -------------------------------------------------------------------------------- /packages/pure_pursuit/include/pure_pursuit/pure_pursuit_node.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "pure_pursuit/pure_pursuit.h" 4 | 5 | #include "geom/msg.h" 6 | #include "model/model.h" 7 | #include "truck_msgs/msg/control.hpp" 8 | #include "truck_msgs/msg/pure_pursuit_status.hpp" 9 | #include "truck_msgs/msg/trajectory.hpp" 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace truck::pure_pursuit { 20 | 21 | class PurePursuitNode : public rclcpp::Node { 22 | public: 23 | PurePursuitNode(); 24 | 25 | private: 26 | void publishCommand(); 27 | 28 | void handleTrajectory(truck_msgs::msg::Trajectory::SharedPtr trajectory); 29 | 30 | void handleOdometry(nav_msgs::msg::Odometry::SharedPtr odometry); 31 | 32 | rclcpp::TimerBase::SharedPtr timer_ = nullptr; 33 | 34 | struct State { 35 | nav_msgs::msg::Odometry::SharedPtr localization_msg = nullptr; 36 | truck_msgs::msg::Trajectory::SharedPtr trajectory_msg = nullptr; 37 | 38 | std::optional trajectory = std::nullopt; 39 | std::optional localization = std::nullopt; 40 | } state_; 41 | 42 | struct Slots { 43 | rclcpp::Subscription::SharedPtr trajectory = nullptr; 44 | rclcpp::Subscription::SharedPtr odometry = nullptr; 45 | } slot_; 46 | 47 | struct Signals { 48 | rclcpp::Publisher::SharedPtr command = nullptr; 49 | rclcpp::Publisher::SharedPtr status = nullptr; 50 | } signal_; 51 | 52 | std::chrono::duration timeout_{0.20}; 53 | std::unique_ptr controller_ = nullptr; 54 | }; 55 | 56 | } // namespace truck::pure_pursuit 57 | -------------------------------------------------------------------------------- /packages/pure_pursuit/include/pure_pursuit/simulator.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "model/model.hpp" 10 | #include "nav_msgs/msg/odometry.hpp" 11 | #include "pure_pursuit/controller.hpp" 12 | #include "pure_pursuit/result.hpp" 13 | 14 | namespace pure_pursuit { 15 | 16 | enum class SimulationError : int8_t { kControllerFailed = 0, kFinishPointNotArrived = 1 }; 17 | 18 | inline std::string errorToString(SimulationError e) { 19 | switch (SimulationError{static_cast(e) & 1}) { 20 | case SimulationError::kControllerFailed: 21 | return "Controller failed. Reason: " 22 | + errorToString(ControllerError{static_cast(e) >> 1}); 23 | case SimulationError::kFinishPointNotArrived: 24 | return "Finish point is not arrived in time"; 25 | default: 26 | return "Unknown error"; 27 | } 28 | } 29 | 30 | using SimulationResult = Result, SimulationError>; 31 | 32 | SimulationResult simulate( 33 | const nav_msgs::msg::Odometry& start, const nav_msgs::msg::Odometry& finish, 34 | uint64_t sim_timeout_ns, uint64_t sim_dt_ns, uint64_t controller_period, 35 | const model::Model& params); 36 | 37 | }; // namespace pure_pursuit 38 | -------------------------------------------------------------------------------- /packages/pure_pursuit/launch/pure_pursuit.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: 3 | name: "model_config" 4 | default: "$(find-pkg-share model)/config/model.yaml" 5 | - arg: 6 | name: "simulation" 7 | default: "false" 8 | - arg: 9 | name: "qos" 10 | default: "0" 11 | - node: 12 | pkg: "pure_pursuit" 13 | exec: "pure_pursuit_node" 14 | name: "pure_pursuit_node" 15 | output: "log" 16 | param: 17 | - { name: "use_sim_time", value: "$(var simulation)" } 18 | - { name: "qos", value: "$(var qos)" } 19 | - { name: "model_config", value: "$(var model_config)" } 20 | - { name: "period", value: 0.05 } 21 | - { name: "radius/min", value: 0.30 } 22 | - { name: "radius/max", value: 2.0 } 23 | - { name: "velocity_factor", value: 2.0 } 24 | - { name: "tolerance", value: 0.05 } 25 | - { name: "max_distance", value: 0.3 } 26 | -------------------------------------------------------------------------------- /packages/pure_pursuit/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | pure_pursuit 5 | 0.0.0 6 | pure_pursuit 7 | Simagin Denis 8 | MIT 9 | 10 | ament_cmake 11 | 12 | common 13 | geom 14 | model 15 | motion 16 | rclcpp 17 | truck_msgs 18 | 19 | ament_cmake_gtest 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /packages/pure_pursuit/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | 3 | #include "pure_pursuit/pure_pursuit_node.h" 4 | 5 | int main(int argc, char** argv) { 6 | rclcpp::init(argc, argv); 7 | rclcpp::spin(std::make_shared()); 8 | rclcpp::shutdown(); 9 | return 0; 10 | } 11 | -------------------------------------------------------------------------------- /packages/route_follower/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(route_follower) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(rclcpp REQUIRED) 6 | find_package(truck_msgs REQUIRED) 7 | find_package(nav_msgs REQUIRED) 8 | find_package(speed REQUIRED) 9 | find_package(collision REQUIRED) 10 | find_package(std_srvs REQUIRED) 11 | find_package(tf2 REQUIRED) 12 | find_package(tf2_ros REQUIRED) 13 | 14 | add_executable(${PROJECT_NAME}_node src/main.cpp src/route_follower_node.cpp) 15 | 16 | ament_target_dependencies( 17 | ${PROJECT_NAME}_node 18 | rclcpp 19 | truck_msgs 20 | nav_msgs 21 | geometry_msgs 22 | speed 23 | collision 24 | tf2 25 | tf2_ros 26 | std_srvs 27 | visualization_msgs 28 | ) 29 | 30 | install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME}) 31 | 32 | target_include_directories( 33 | ${PROJECT_NAME}_node 34 | PUBLIC "$" 35 | "$" 36 | "$" 37 | ) 38 | 39 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 40 | 41 | ament_package() 42 | -------------------------------------------------------------------------------- /packages/route_follower/launch/route_follower.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: 3 | name: "model_config" 4 | default: "$(find-pkg-share model)/config/model.yaml" 5 | - arg: 6 | name: "simulation" 7 | default: "false" 8 | - node: 9 | pkg: "route_follower" 10 | exec: "route_follower_node" 11 | name: "route_follower_node" 12 | output: "log" 13 | param: 14 | - { name: "use_sim_time", value: "$(var simulation)" } 15 | - { name: "model_config", value: "$(var model_config)" } 16 | - { name: "period", value: 0.1 } 17 | - { name: "resolution", value: 0.02 } 18 | - { name: "check_in_distance", value: 0.30 } 19 | - { name: "safety_margin", value: 0.15 } 20 | - { name: "distance_to_obstacle", value: 1.5 } 21 | - { name: "acceleration/min", value: -0.5 } 22 | - { name: "acceleration/max", value: 0.3 } 23 | -------------------------------------------------------------------------------- /packages/route_follower/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | route_follower 5 | 0.0.0 6 | route_follower 7 | xs1nus 8 | MIT 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | truck_msgs 14 | nav_msgs 15 | geometry_msgs 16 | speed 17 | collision 18 | tf2 19 | tf2_ros 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /packages/route_follower/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "route_follower/route_follower_node.h" 2 | #include 3 | 4 | int main(int argc, char** argv) { 5 | rclcpp::init(argc, argv); 6 | rclcpp::spin(std::make_shared()); 7 | rclcpp::shutdown(); 8 | return 0; 9 | } 10 | -------------------------------------------------------------------------------- /packages/routing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(routing) 3 | 4 | add_compile_options(-Wall -Wextra -Wpedantic) 5 | 6 | find_package(ament_cmake REQUIRED) 7 | find_package(rclcpp REQUIRED) 8 | find_package(map REQUIRED) 9 | find_package(navigation REQUIRED) 10 | find_package(truck_msgs REQUIRED) 11 | find_package(nav_msgs REQUIRED) 12 | find_package(geometry_msgs REQUIRED) 13 | find_package(Boost REQUIRED) 14 | find_package(tf2_ros REQUIRED) 15 | 16 | add_executable(${PROJECT_NAME}_node src/main.cpp src/routing_node.cpp) 17 | 18 | ament_target_dependencies( 19 | ${PROJECT_NAME}_node 20 | rclcpp 21 | map 22 | navigation 23 | truck_msgs 24 | nav_msgs 25 | geometry_msgs 26 | Boost 27 | tf2_ros 28 | ) 29 | 30 | install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME}) 31 | 32 | target_include_directories( 33 | ${PROJECT_NAME}_node 34 | PUBLIC "$" 35 | "$" 36 | "$" 37 | ) 38 | 39 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 40 | 41 | ament_package() 42 | -------------------------------------------------------------------------------- /packages/routing/launch/routing.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: 3 | name: "map_config" 4 | default: "$(find-pkg-share map)/data/map_6.geojson" 5 | - node: 6 | pkg: "routing" 7 | exec: "routing_node" 8 | name: "routing_node" 9 | output: "log" 10 | param: 11 | - { name: "map_config", value: "$(var map_config)" } 12 | 13 | - name: "route" 14 | param: 15 | - { name: "max_ego_dist", value: 2.0 } 16 | - { name: "postfix_len", value: 2.0 } 17 | - { name: "spline_step", value: 0.2 } 18 | 19 | - name: "mesh" 20 | param: 21 | - { name: "dist", value: 2.8 } 22 | - { name: "offset", value: 1.2 } 23 | 24 | - name: "graph" 25 | param: 26 | - { name: "k_nearest_mode", value: false } 27 | - { name: "k_nearest", value: 15 } 28 | - { name: "search_radius", value: 7.0 } 29 | -------------------------------------------------------------------------------- /packages/routing/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | routing 5 | 0.0.0 6 | routing 7 | apmilko 8 | MIT 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | map 14 | navigation 15 | truck_msgs 16 | nav_msgs 17 | geometry_msgs 18 | Boost 19 | tf2_ros 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /packages/routing/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "routing/routing_node.h" 2 | 3 | #include 4 | 5 | int main(int argc, char* argv[]) { 6 | rclcpp::init(argc, argv); 7 | rclcpp::spin(std::make_shared()); 8 | rclcpp::shutdown(); 9 | return 0; 10 | } 11 | -------------------------------------------------------------------------------- /packages/simulator_2d/include/simulator_2d/simulation_map.h: -------------------------------------------------------------------------------- 1 | #include "geom/pose.h" 2 | #include "geom/polygon.h" 3 | #include "geom/segment.h" 4 | #include "geom/boost/point.h" 5 | #include "geom/boost/segment.h" 6 | #include "geom/boost/box.h" 7 | #include "model/model.h" 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | namespace truck::simulator { 15 | 16 | namespace bg = boost::geometry; 17 | namespace bgi = boost::geometry::index; 18 | 19 | using IndexSegment = std::pair; 20 | using IndexSegments = std::vector; 21 | using RTree = bgi::rtree>; 22 | 23 | class SimulationMap { 24 | public: 25 | void resetMap(const std::string& path); 26 | void eraseMap(); 27 | const geom::Segments& obstacles() const noexcept { return obstacles_; } 28 | const RTree& rtree() const noexcept { return rtree_; } 29 | 30 | private: 31 | void initializeRTree(); 32 | 33 | geom::Segments obstacles_; 34 | RTree rtree_; 35 | }; 36 | 37 | bool hasCollision(const SimulationMap& map, const geom::Polygon& shape_polygon, double precision); 38 | std::vector getLidarRanges( 39 | const SimulationMap& map, const geom::Pose& lidar_pose, const model::Lidar& lidar, 40 | double precision); 41 | 42 | } // namespace truck::simulator 43 | -------------------------------------------------------------------------------- /packages/simulator_2d/include/simulator_2d/status_code.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace truck::simulator { 4 | 5 | enum class StatusCode { IN_PROGRESS, OK, COLLISION }; 6 | 7 | } // namespace truck::simulator 8 | -------------------------------------------------------------------------------- /packages/simulator_2d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | simulator_2d 5 | 0.0.1 6 | The 2D physics simulator based on Ackerman kinematics. 7 | root 8 | MIT 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | model 14 | nav_msgs 15 | truck_msgs 16 | geom 17 | tf2_msgs 18 | tf2_ros 19 | rosgraph_msgs 20 | control_proxy 21 | visualization 22 | nlohmann_json 23 | map 24 | sensor_msgs 25 | Boost 26 | 27 | ament_cmake_gtest 28 | rosbag2_cpp 29 | rosbag2_storage 30 | truck_msgs 31 | 32 | 33 | ament_cmake 34 | 35 | 36 | -------------------------------------------------------------------------------- /packages/simulator_2d/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "simulator_node.h" 2 | 3 | #include 4 | 5 | int main(int argc, char* argv[]) { 6 | rclcpp::init(argc, argv); 7 | rclcpp::spin(std::make_shared()); 8 | rclcpp::shutdown(); 9 | return 0; 10 | } 11 | -------------------------------------------------------------------------------- /packages/simulator_2d/test/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char* argv[]) { 4 | ::testing::InitGoogleTest(&argc, argv); 5 | return RUN_ALL_TESTS(); 6 | } 7 | -------------------------------------------------------------------------------- /packages/simulator_2d/test/simulation_map_tests.cpp: -------------------------------------------------------------------------------- 1 | #include "simulator_2d/simulation_map.h" 2 | 3 | #include 4 | 5 | #include 6 | 7 | using namespace truck::simulator; 8 | 9 | const std::string kMapPkgPath = ament_index_cpp::get_package_share_directory("map"); 10 | 11 | TEST(SimulationMap, hasCollision) { 12 | constexpr double precision = 1e-9; 13 | 14 | { 15 | // Arrange. 16 | SimulationMap map; 17 | map.resetMap(kMapPkgPath + "/data/map_6.geojson"); 18 | const auto shape = truck::geom::Polygon{{20, 38}, {19, 38}, {20, 37}, {19, 37}}; 19 | 20 | // Act. 21 | const auto result = hasCollision(map, shape, precision); 22 | 23 | // Assert. 24 | EXPECT_EQ(result, false); 25 | } 26 | 27 | { 28 | // Arrange. 29 | SimulationMap map; 30 | map.resetMap(kMapPkgPath + "/data/map_6.geojson"); 31 | const auto shape = truck::geom::Polygon{{25, 32}, {27, 32}, {27, 29}, {25, 29}}; 32 | 33 | // Act. 34 | const auto result = hasCollision(map, shape, precision); 35 | 36 | // Assert. 37 | EXPECT_EQ(result, true); 38 | } 39 | 40 | { 41 | // Arrange. 42 | SimulationMap map; 43 | map.resetMap(kMapPkgPath + "/data/map_6.geojson"); 44 | const auto shape = truck::geom::Polygon{ 45 | {22.133002182962517, 31.000984165385418}, {22, 30}, {21, 31}, {21, 30}}; 46 | 47 | // Act. 48 | const auto result = hasCollision(map, shape, precision); 49 | 50 | // Assert. 51 | EXPECT_EQ(result, true); 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /packages/speed/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(speed) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(model REQUIRED) 6 | find_package(motion REQUIRED) 7 | find_package(Boost REQUIRED) 8 | 9 | add_library(${PROJECT_NAME} SHARED src/greedy_planner.cpp) 10 | 11 | ament_target_dependencies(${PROJECT_NAME} common model motion Boost) 12 | 13 | target_include_directories( 14 | ${PROJECT_NAME} 15 | PUBLIC "$" 16 | "$" 17 | ) 18 | 19 | ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) 20 | ament_export_dependencies(Boost model motion) 21 | 22 | install( 23 | TARGETS ${PROJECT_NAME} 24 | EXPORT ${PROJECT_NAME} 25 | ARCHIVE DESTINATION lib 26 | LIBRARY DESTINATION lib 27 | RUNTIME DESTINATION bin 28 | ) 29 | 30 | install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) 31 | 32 | ament_package() 33 | -------------------------------------------------------------------------------- /packages/speed/include/speed/greedy_planner.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "common/math.h" 4 | #include "model/model.h" 5 | #include "motion/trajectory.h" 6 | 7 | namespace truck::speed { 8 | 9 | class GreedyPlanner { 10 | public: 11 | struct Params { 12 | Limits acceleration{-0.5, 0.2}; 13 | double distance_to_obstacle = 0.7; 14 | }; 15 | 16 | GreedyPlanner(const Params& params, const model::Model& model); 17 | 18 | GreedyPlanner& setScheduledVelocity(double scheduled_velocity) { 19 | const Limits limit{.0, model_.baseVelocityLimits().max}; 20 | 21 | VERIFY_FMT( 22 | limit.isMet(scheduled_velocity), 23 | "Scheduled velocity %f not in [%f, %f], backward motion is not supported!", 24 | scheduled_velocity, 25 | limit.min, 26 | limit.max); 27 | 28 | scheduled_velocity_ = scheduled_velocity; 29 | 30 | return *this; 31 | } 32 | 33 | void fill(motion::Trajectory& trajectory) const; 34 | 35 | private: 36 | Params params_; 37 | model::Model model_; 38 | double scheduled_velocity_ = 0.0; 39 | }; 40 | 41 | } // namespace truck::speed 42 | -------------------------------------------------------------------------------- /packages/speed/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | speed 5 | 0.0.0 6 | speed 7 | Simagin Denis 8 | MIT 9 | 10 | ament_cmake 11 | 12 | Boost 13 | model 14 | motion 15 | truck_msgs 16 | 17 | 18 | ament_cmake 19 | 20 | 21 | -------------------------------------------------------------------------------- /packages/svg_debug_drawer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(svg_debug_drawer) 3 | 4 | add_compile_options(-Wall -Wextra -Wpedantic -Werror) 5 | 6 | find_package(ament_cmake REQUIRED) 7 | find_package(common REQUIRED) 8 | find_package(geom REQUIRED) 9 | find_package(pugixml REQUIRED) 10 | find_package(fmt REQUIRED) 11 | 12 | add_library(${PROJECT_NAME} SHARED src/sdd.cpp) 13 | 14 | ament_target_dependencies(${PROJECT_NAME} common geom pugixml fmt) 15 | 16 | target_link_libraries(${PROJECT_NAME} pugixml fmt) 17 | 18 | target_include_directories( 19 | ${PROJECT_NAME} 20 | PUBLIC "$" 21 | "$" 22 | ) 23 | 24 | ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) 25 | ament_export_dependencies(common geom pugixml fmt) 26 | 27 | install( 28 | TARGETS ${PROJECT_NAME} 29 | EXPORT ${PROJECT_NAME} 30 | ARCHIVE DESTINATION lib 31 | LIBRARY DESTINATION lib 32 | RUNTIME DESTINATION bin 33 | ) 34 | 35 | install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) 36 | 37 | if(BUILD_TESTING) 38 | find_package(ament_cmake_gtest REQUIRED) 39 | 40 | ament_add_gtest( 41 | ${PROJECT_NAME}_tests test/main.cpp WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} 42 | ) 43 | 44 | target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME}) 45 | endif() 46 | 47 | ament_package() 48 | -------------------------------------------------------------------------------- /packages/svg_debug_drawer/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | svg_debug_drawer 5 | 0.0.0 6 | svg_debug_drawer 7 | Nikita Romanov 8 | MIT 9 | 10 | ament_cmake 11 | common 12 | geom 13 | pugixml 14 | fmt 15 | 16 | -------------------------------------------------------------------------------- /packages/svg_debug_drawer/test/data/im1_in.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 1 6 | 7 | 8 | 9 | 2 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /packages/truck/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(truck) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) 7 | 8 | ament_package() 9 | -------------------------------------------------------------------------------- /packages/truck/README.md: -------------------------------------------------------------------------------- 1 | # truck 2 | 3 | | [**docs**](../../doc/README.md) | [**packages**](../README.md) | 4 | |---------------------------------|------------------------------| 5 | 6 | Main package that brings all together. 7 | 8 | Usage: 9 | - `ros2 launch truck truck.yaml` - run full pipeline 10 | - `ros2 launch truck assets.yaml` - prepare all assets for gzweb (need run once befor simulation) 11 | - `ros2 launch truck simulator.yaml` - run simulation (gzserver + gzweb + part of pipeline) 12 | -------------------------------------------------------------------------------- /packages/truck/config/point_matcher.yaml: -------------------------------------------------------------------------------- 1 | readingDataPointsFilters: 2 | - MinDistDataPointsFilter: 3 | minDist: 0.1 4 | - MaxDistDataPointsFilter: 5 | maxDist: 40.0 6 | 7 | referenceDataPointsFilters: 8 | - MinDistDataPointsFilter: 9 | minDist: 0.1 10 | - MaxDistDataPointsFilter: 11 | maxDist: 40.0 12 | 13 | matcher: 14 | KDTreeMatcher: 15 | knn: 1 16 | epsilon: 0 17 | 18 | outlierFilters: 19 | - TrimmedDistOutlierFilter: 20 | ratio: 0.75 21 | 22 | errorMinimizer: 23 | PointToPlaneErrorMinimizer 24 | 25 | transformationCheckers: 26 | - CounterTransformationChecker: 27 | maxIterationCount: 100 28 | - DifferentialTransformationChecker: 29 | minDiffRotErr: 0.001 30 | minDiffTransErr: 0.01 31 | smoothLength: 4 32 | 33 | inspector: 34 | NullInspector 35 | 36 | logger: 37 | NullLogger 38 | -------------------------------------------------------------------------------- /packages/truck/launch/bridge.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - node: 3 | pkg: "foxglove_bridge" 4 | exec: "foxglove_bridge" 5 | name: "foxglove_bridge" 6 | output: "log" 7 | -------------------------------------------------------------------------------- /packages/truck/launch/camera.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - node: 3 | pkg: "realsense2_camera" 4 | exec: "realsense2_camera_node" 5 | name: "camera" 6 | namespace: "camera" 7 | output: "log" 8 | 9 | param: 10 | - { name: "initial_reset", value: True } 11 | - { name: "enable_sync", value: False } 12 | - { name: "publish_tf", value: False } 13 | 14 | - { name: "enable_color", value: False } 15 | - { name: "rgb_camera.profile", value: "848x480x30" } 16 | 17 | - { name: "enable_depth", value: False } 18 | - { name: "depth_module.profile", value: "424x240x15" } 19 | 20 | - { name: "enable_accel", value: True } 21 | - { name: "accel_fps", value: 200 } 22 | 23 | - { name: "enable_gyro", value: True } 24 | - { name: "gyro_fps", value: 200 } 25 | 26 | - { name: "unite_imu_method", value: 1 } 27 | - { name: "hold_back_imu_for_frames", value: False } 28 | 29 | - { name: "enable_fisheye", value: False } 30 | - { name: "enable_fisheye1", value: False } 31 | - { name: "enable_fisheye2", value: False } 32 | - { name: "enable_infra", value: False } 33 | - { name: "enable_infra1", value: False } 34 | - { name: "enable_infra2", value: False } 35 | -------------------------------------------------------------------------------- /packages/truck/launch/common.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: { name: "simulation", default: "false" } 3 | - arg: { name: "qos", default: "0" } 4 | - arg: { name: "map_config", default: "$(find-pkg-share map)/data/map_6.geojson" } 5 | 6 | - include: 7 | file: $(find-pkg-share model)/launch/model_tf.yaml 8 | arg: 9 | - { name: "simulation", value: "$(var simulation)" } 10 | 11 | - include: 12 | file: $(find-pkg-share truck)/launch/control.yaml 13 | arg: 14 | - { name: "simulation", value: "$(var simulation)" } 15 | - { name: "qos", value: "$(var qos)" } 16 | 17 | - include: 18 | file: $(find-pkg-share pure_pursuit)/launch/pure_pursuit.yaml 19 | arg: 20 | - { name: "simulation", value: "$(var simulation)" } 21 | - { name: "qos", value: "$(var qos)" } 22 | 23 | - include: 24 | file: $(find-pkg-share visualization)/launch/visualization.yaml 25 | arg: 26 | - { name: "simulation", value: "$(var simulation)" } 27 | - { name: "qos", value: "$(var qos)" } 28 | - { name: "map_config", value: "$(var map_config)" } 29 | 30 | - include: 31 | file: $(find-pkg-share truck)/launch/slam.yaml 32 | arg: 33 | - { name: "simulation", value: "$(var simulation)" } 34 | - { name: "qos", value: "$(var qos)" } 35 | 36 | - include: 37 | file: $(find-pkg-share occupancy_grid)/launch/occupancy_grid.yaml 38 | arg: 39 | - { name: "simulation", value: "$(var simulation)" } 40 | - { name: "qos", value: "$(var qos)" } 41 | 42 | - include: 43 | file: $(find-pkg-share truck)/launch/bridge.yaml 44 | -------------------------------------------------------------------------------- /packages/truck/launch/control.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: { name: "simulation", default: "false" } 3 | 4 | - include: 5 | file: $(find-pkg-share control_proxy)/launch/control_proxy.yaml 6 | arg: 7 | - { name: "simulation", value: $(var simulation) } 8 | -------------------------------------------------------------------------------- /packages/truck/launch/demo.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: { name: "simulation", default: "$(env TRUCK_SIMULATION)" } 3 | 4 | - let: { name: "env", value: "simulator", if: "$(var simulation)" } 5 | - let: { name: "env", value: "truck", unless: "$(var simulation)" } 6 | 7 | - include: 8 | file: $(find-pkg-share truck)/launch/$(var env).yaml 9 | 10 | - include: 11 | file: $(find-pkg-share waypoint_follower)/launch/waypoint_follower.yaml 12 | arg: 13 | - { name: "simulation", value: $(var simulation) } 14 | -------------------------------------------------------------------------------- /packages/truck/launch/graph.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: { name: "simulation", default: "$(env TRUCK_SIMULATION)" } 3 | 4 | - let: { name: "env", value: "simulator", if: "$(var simulation)" } 5 | - let: { name: "env", value: "truck", unless: "$(var simulation)" } 6 | 7 | - include: 8 | file: $(find-pkg-share truck)/launch/$(var env).yaml 9 | 10 | - include: 11 | file: $(find-pkg-share planner)/launch/planner.yaml 12 | -------------------------------------------------------------------------------- /packages/truck/launch/joy.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: 3 | name: "simulation" 4 | default: "false" 5 | - node: 6 | pkg: "joy" 7 | exec: "joy_node" 8 | name: "joy_node" 9 | output: "log" 10 | param: 11 | - { name: "use_sim_time", value: "$(var simulation)" } 12 | - { name: "coalesce_interval_ms", value: 20 } 13 | -------------------------------------------------------------------------------- /packages/truck/launch/lidar.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - node: 3 | pkg: "sllidar_ros2" 4 | exec: "sllidar_node" 5 | name: "lidar" 6 | namespace: "lidar" 7 | output: "log" 8 | 9 | param: 10 | - { name: "serial_port", value: "/dev/ttyUSB0" } 11 | - { name: "serial_baudrate", value: 1000000 } 12 | - { name: "frame_id", value: "lidar_link" } 13 | - { name: "inverted", value: False } 14 | - { name: "angle_compensate", value: True } 15 | - { name: "scan_mode", value: "DenseBoost" } 16 | - { name: "scan_frequency", value: 10.0 } 17 | - { name: "rpm_speed", value: 600 } 18 | -------------------------------------------------------------------------------- /packages/truck/launch/remote.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - include: 3 | file: $(find-pkg-share truck)/launch/control.yaml 4 | 5 | - include: 6 | file: $(find-pkg-share hardware_node)/launch/hardware_node.yaml 7 | -------------------------------------------------------------------------------- /packages/truck/launch/route_follower_demo.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: { name: "simulation", default: "$(env TRUCK_SIMULATION)" } 3 | - arg: { name: "map_config", default: "$(find-pkg-share map)/data/map_6.geojson" } 4 | 5 | 6 | - let: { name: "env", value: "simulator", if: "$(var simulation)" } 7 | - let: { name: "env", value: "truck", unless: "$(var simulation)" } 8 | 9 | - include: 10 | file: $(find-pkg-share truck)/launch/$(var env).yaml 11 | 12 | - include: 13 | file: $(find-pkg-share route_follower)/launch/route_follower.yaml 14 | arg: 15 | - { name: "simulation", value: $(var simulation) } 16 | 17 | - include: 18 | file: $(find-pkg-share routing)/launch/routing.yaml 19 | arg: 20 | - { name: "map_config", value: "$(var map_config)" } 21 | -------------------------------------------------------------------------------- /packages/truck/launch/routing_demo.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: { name: "simulation", default: "$(env TRUCK_SIMULATION)" } 3 | 4 | - let: { name: "env", value: "simulator", if: "$(var simulation)" } 5 | - let: { name: "env", value: "truck", unless: "$(var simulation)" } 6 | 7 | - include: 8 | file: $(find-pkg-share truck)/launch/$(var env).yaml 9 | 10 | - include: 11 | file: $(find-pkg-share routing)/launch/routing.yaml 12 | -------------------------------------------------------------------------------- /packages/truck/launch/simulator.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: { name: "model_config", default: "$(find-pkg-share model)/config/model.yaml" } 3 | - arg: { name: "map_config", default: "$(find-pkg-share map)/data/map_6.geojson" } 4 | - arg: { name: "initial_x", default: "5.0" } 5 | - arg: { name: "initial_y", default: "4.0" } 6 | - arg: { name: "initial_yaw", default: "0.3" } 7 | 8 | - include: 9 | file: $(find-pkg-share simulator_2d)/launch/simulator_2d.yaml 10 | arg: 11 | - { name: "model_config", value: "$(var model_config)" } 12 | - { name: "map_config", value: "$(var map_config)" } 13 | - { name: "initial_x", value: "$(var initial_x)" } 14 | - { name: "initial_y", value: "$(var initial_y)" } 15 | - { name: "initial_yaw", value: "$(var initial_yaw)" } 16 | 17 | - include: 18 | file: $(find-pkg-share truck)/launch/common.yaml 19 | arg: 20 | - { name: "simulation", value: "true" } 21 | - { name: "qos", value: "2" } 22 | - { name: "map_config", value: "$(var map_config)" } 23 | -------------------------------------------------------------------------------- /packages/truck/launch/truck.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - include: 3 | file: $(find-pkg-share truck)/launch/lidar.yaml 4 | 5 | - include: 6 | file: $(find-pkg-share truck)/launch/camera.yaml 7 | 8 | - include: 9 | file: $(find-pkg-share hardware_node)/launch/hardware_node.yaml 10 | 11 | - include: 12 | file: $(find-pkg-share truck)/launch/common.yaml 13 | arg: 14 | - { name: "simulation", value: "false" } 15 | - { name: "qos", value: "0" } 16 | 17 | - include: 18 | file: $(find-pkg-share perf)/launch/perf_stat.yaml 19 | 20 | - include: 21 | file: $(find-pkg-share video_streaming)/launch/video_streaming.yaml 22 | -------------------------------------------------------------------------------- /packages/truck/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | truck 4 | 0.1.0 5 | Truck launch package 6 | Simagin Denis 7 | MIT 8 | 9 | ament_cmake 10 | 11 | launch_ros 12 | realsense2_camera 13 | ros_environment 14 | 15 | control_proxy 16 | icp_odometry 17 | hardware_node 18 | occupancy_grid 19 | pure_pursuit 20 | visualization 21 | perf 22 | video_streaming 23 | waypoint_follower 24 | simulator_2d 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /packages/truck_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(truck_msgs) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | find_package(geometry_msgs REQUIRED) 7 | find_package(nav_msgs REQUIRED) 8 | find_package(visualization_msgs REQUIRED) 9 | find_package(builtin_interfaces REQUIRED) 10 | find_package(rosidl_default_generators REQUIRED) 11 | 12 | rosidl_generate_interfaces( 13 | ${PROJECT_NAME} 14 | msg/Control.msg 15 | msg/ControlMode.msg 16 | msg/RemoteControl.msg 17 | msg/CpuStat.msg 18 | msg/HardwareStatus.msg 19 | msg/HardwareTelemetry.msg 20 | msg/IcpOdometryStat.msg 21 | msg/MemStat.msg 22 | msg/MetaData.msg 23 | msg/NavigationMesh.msg 24 | msg/NavigationRoute.msg 25 | msg/PerfStat.msg 26 | msg/Point.msg 27 | msg/PurePursuitStatus.msg 28 | msg/SimulationState.msg 29 | msg/Trajectory.msg 30 | msg/TrajectoryState.msg 31 | msg/Waypoints.msg 32 | DEPENDENCIES 33 | builtin_interfaces 34 | geometry_msgs 35 | nav_msgs 36 | visualization_msgs 37 | ADD_LINTER_TESTS 38 | ) 39 | 40 | ament_export_dependencies(rosidl_default_runtime) 41 | 42 | ament_package() 43 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/Control.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | float64 curvature 4 | float64 velocity 5 | bool has_acceleration 6 | float64 acceleration 7 | 8 | # for debug 9 | bool has_target 10 | geometry_msgs/PointStamped target 11 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/ControlMode.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | uint8 OFF = 0 4 | uint8 REMOTE = 1 5 | uint8 AUTO = 2 6 | 7 | uint8 mode 8 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/CpuStat.msg: -------------------------------------------------------------------------------- 1 | int16 id -1 # -1 means aggregation 2 | float32 usage 0 3 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/HardwareStatus.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | bool armed # Whether if ODrive and steering are currently activated 3 | string[] errors # List of ODrive errors (e.g. "axis.WATCHDOG_TIMER_EXPIRED") 4 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/HardwareTelemetry.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | # CORE 4 | float64 current_rps # RPS 5 | float64 target_rps # RPS 6 | float64 battery_voltage # Volts 7 | float64 battery_current # Amps 8 | 9 | # WHEELS 10 | float64 target_left_steering # rad 11 | float64 current_left_steering # rad 12 | float64 target_right_steering # rad 13 | float64 current_right_steering # rad 14 | 15 | float64 rear_left_wheel_velocity # rad/s 16 | float64 rear_right_wheel_velocity # rad/s 17 | float64 front_left_wheel_velocity # rad/s 18 | float64 front_right_wheel_velocity # rad/s 19 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/IcpOdometryStat.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | builtin_interfaces/Duration latency 4 | 5 | float32 x 6 | float32 y 7 | float32 theta 8 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/MemStat.msg: -------------------------------------------------------------------------------- 1 | uint64 total 0 # KB 2 | uint64 free 0 # KB 3 | float32 usage 0 4 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/MetaData.msg: -------------------------------------------------------------------------------- 1 | uint64 created_at 2 | string[] debug 3 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/NavigationMesh.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point[] data 2 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/NavigationRoute.msg: -------------------------------------------------------------------------------- 1 | uint64 postfix_index 2 | geometry_msgs/Point[] data 3 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/PerfStat.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | CpuStat[] cpu 3 | MemStat mem 4 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/Point.msg: -------------------------------------------------------------------------------- 1 | float64 x 2 | float64 y 3 | float64 theta 4 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/PurePursuitStatus.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | # STATUS 4 | uint8 OK = 200 5 | uint8 NO_LOCALIZATION = 201 6 | uint8 NO_TRAJECTORY = 201 7 | uint8 ERROR = 203 8 | 9 | uint8 status 10 | 11 | # ERROR 12 | uint8 UNKNOWN = 0 13 | uint8 NO_PROJECTION = 1 14 | uint8 UNREACHABLE_PROJECTION = 2 15 | uint8 IMPOSSIBLE_BUILD_ARC = 3 16 | 17 | uint8 error 18 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/RandomSeed.msg: -------------------------------------------------------------------------------- 1 | uint64 seed 2 | float64 probability 3 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/RemoteControl.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | uint8 OFF = 0 4 | uint8 REMOTE = 1 5 | uint8 AUTO = 2 6 | 7 | uint8 mode 8 | # values in range [-1, 1] 9 | float64 curvature_ratio 10 | float64 velocity_ratio 11 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/SimulationState.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | float64 speed 4 | float64 steering 5 | bool collision 6 | 7 | geometry_msgs/Pose pose 8 | geometry_msgs/Vector3 gyro_angular_velocity 9 | geometry_msgs/Vector3 accel_linear_acceleration 10 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/Trajectory.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | bool overbraking 4 | 5 | TrajectoryState[] states 6 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/TrajectoryState.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose pose 2 | 3 | float64 margin 4 | bool collision 5 | 6 | bool reachable 7 | float64 time 8 | 9 | float64 distance 10 | float64 velocity 11 | float64 acceleration 12 | -------------------------------------------------------------------------------- /packages/truck_msgs/msg/Waypoints.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | geometry_msgs/Point[] waypoints 3 | -------------------------------------------------------------------------------- /packages/truck_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | truck_msgs 5 | 0.0.0 6 | truck_msgs 7 | Simagin Denis 8 | MIT 9 | 10 | ament_cmake 11 | 12 | rosidl_default_generators 13 | rosidl_default_runtime 14 | rosidl_interface_packages 15 | 16 | geometry_msgs 17 | nav_msgs 18 | visualization_msgs 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /packages/video_streaming/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(video_streaming) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 7 | install(FILES mediamtx.yaml DESTINATION share/${PROJECT_NAME}) 8 | 9 | ament_package() 10 | -------------------------------------------------------------------------------- /packages/video_streaming/launch/video_streaming.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - executable: 3 | name: "video_streaming" 4 | cmd: "mediamtx $(find-pkg-share video_streaming)/mediamtx.yaml" 5 | output: "screen" 6 | shell: True 7 | -------------------------------------------------------------------------------- /packages/video_streaming/mediamtx.yaml: -------------------------------------------------------------------------------- 1 | logLevel: info 2 | logDestinations: [stdout] 3 | 4 | rtmpDisable: yes 5 | hlsDisable: yes 6 | rtspAddress: :8554 7 | webrtcAddress: :8889 8 | 9 | paths: 10 | cam: 11 | source: publisher 12 | runOnDemandRestart: yes 13 | runOnDemandCloseAfter: 10s 14 | runOnDemand: > 15 | gst-launch-1.0 16 | nvv4l2camerasrc device=/dev/video4 17 | ! 'video/x-raw(memory:NVMM), format=YUY2, width=640, height=480, framerate=30/1' 18 | ! nvvidconv ! nvv4l2vp9enc bitrate=400000 maxperf-enable=true 19 | ! rtspclientsink location=rtsp://localhost:$RTSP_PORT/$RTSP_PATH 20 | -------------------------------------------------------------------------------- /packages/video_streaming/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | video_streaming 5 | 0.1.0 6 | Live video streaming server based on MediaMTX + Nvidia Accelerated GStreamer 7 | AndBondStyle 8 | MIT 9 | 10 | ament_cmake 11 | 12 | 13 | ament_cmake 14 | 15 | 16 | -------------------------------------------------------------------------------- /packages/visualization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(visualization) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(rclcpp REQUIRED) 6 | find_package(map REQUIRED) 7 | find_package(geom REQUIRED) 8 | find_package(model REQUIRED) 9 | find_package(nav_msgs REQUIRED) 10 | find_package(std_msgs REQUIRED) 11 | find_package(truck_msgs REQUIRED) 12 | find_package(visualization_msgs REQUIRED) 13 | find_package(tf2_geometry_msgs REQUIRED) 14 | find_package(Boost REQUIRED) 15 | 16 | add_library(${PROJECT_NAME} SHARED src/color.cpp src/msg.cpp) 17 | 18 | ament_target_dependencies( 19 | ${PROJECT_NAME} geom visualization_msgs std_msgs truck_msgs Boost 20 | ) 21 | 22 | target_include_directories( 23 | ${PROJECT_NAME} 24 | PUBLIC "$" 25 | "$" 26 | ) 27 | 28 | ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) 29 | 30 | ament_export_dependencies(geom visualization_msgs std_msgs truck_msgs Boost) 31 | 32 | install( 33 | TARGETS ${PROJECT_NAME} 34 | EXPORT ${PROJECT_NAME} 35 | ARCHIVE DESTINATION lib 36 | LIBRARY DESTINATION lib 37 | RUNTIME DESTINATION bin 38 | ) 39 | 40 | install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) 41 | 42 | add_executable(${PROJECT_NAME}_node src/main.cpp src/visualization_node.cpp) 43 | 44 | ament_target_dependencies( 45 | ${PROJECT_NAME}_node 46 | rclcpp 47 | map 48 | model 49 | nav_msgs 50 | truck_msgs 51 | visualization_msgs 52 | tf2_geometry_msgs 53 | ) 54 | 55 | target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME}) 56 | 57 | install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME}) 58 | 59 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 60 | 61 | ament_package() 62 | -------------------------------------------------------------------------------- /packages/visualization/README.md: -------------------------------------------------------------------------------- 1 | # Visualization 2 | 3 | ## Overview 4 | The node publishes all visualization topics. Color encoding for control mode: 5 | - `off` - red 6 | - `blue` - remote 7 | - `green` - green 8 | 9 | ## Parameters 10 | - `model_config` - path to model yaml file (see model module) 11 | 12 | ## Topics 13 | ### Input 14 | - `/control/mode` [[truck_interfecase/ControlMode](https://github.com/robotics-laboratory/truck/blob/master/packages/truck_msgs/msg/ControlMode.msg)] - control mode 15 | - `/control/command` [[truck_interfecase/Control](https://github.com/robotics-laboratory/truck/blob/master/packages/truck_msgs/msg/Control.msg)] - target of trajectory controller 16 | - `/waypoints` [[truck_interfecase/Control](https://github.com/robotics-laboratory/truck/blob/master/packages/truck_msgs/msg/Waypoints.msg)] - target waypoints to pass through 17 | - `/ekf/odom/filtered` [[nav_msgs/Odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html)] - localization in odometric world 18 | 19 | ### Output 20 | - `/visualization/ego` [[visualization_msgs/Marker](http://docs.ros.org/en/melodic/api/visualization_msgs/html/msg/Marker.html)] - pose in odometric world (color means control mode). 21 | - `/visualization/arc`[[visualization_msgs/Marker](http://docs.ros.org/en/melodic/api/visualization_msgs/html/msg/Marker.html)] - ccontrol command (curvature and velocity). 22 | -------------------------------------------------------------------------------- /packages/visualization/include/visualization/color.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "truck_msgs/msg/control_mode.hpp" 4 | 5 | #include 6 | 7 | namespace truck::visualization::color { 8 | 9 | std_msgs::msg::ColorRGBA make(float r, float g, float b, float a = 1.0); 10 | std_msgs::msg::ColorRGBA make(const std::vector& rbga_color); 11 | 12 | std_msgs::msg::ColorRGBA white(float alpha = 1.0); 13 | std_msgs::msg::ColorRGBA gray(float alpha = 1.0); 14 | std_msgs::msg::ColorRGBA red(float alpha = 1.0); 15 | std_msgs::msg::ColorRGBA green(float alpha = 1.0); 16 | std_msgs::msg::ColorRGBA blue(float alpha = 1.0); 17 | 18 | std_msgs::msg::ColorRGBA make(const truck_msgs::msg::ControlMode& mode); 19 | 20 | std_msgs::msg::ColorRGBA plasma(float x, float alpha = 1.0); 21 | 22 | } // namespace truck::visualization::color 23 | -------------------------------------------------------------------------------- /packages/visualization/include/visualization/msg.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geom/complex_polygon.h" 4 | 5 | #include 6 | 7 | namespace truck::visualization { 8 | 9 | namespace msg { 10 | 11 | visualization_msgs::msg::Marker toMarker( 12 | const geom::ComplexPolygon& complex_polygon, std::string frame_id = "world", 13 | std::vector rgba_color = {0.3, 0.3, 0.3, 1.0}, float z_lev = 1.0); 14 | 15 | } // namespace msg 16 | } // namespace truck::visualization 17 | -------------------------------------------------------------------------------- /packages/visualization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | visualization 5 | 0.0.0 6 | visualization 7 | Simagin Denis 8 | MIT 9 | 10 | ament_cmake 11 | ros2launch 12 | 13 | rclcpp 14 | map 15 | geom 16 | model 17 | nav_msgs 18 | std_msgs 19 | truck_msgs 20 | visualization_msgs 21 | tf2_geometry_msgs 22 | Boost 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /packages/visualization/src/color.cpp: -------------------------------------------------------------------------------- 1 | #include "visualization/color.h" 2 | 3 | #include 4 | 5 | namespace truck::visualization::color { 6 | 7 | std_msgs::msg::ColorRGBA make(float r, float g, float b, float a) { 8 | std_msgs::msg::ColorRGBA color; 9 | color.r = r; 10 | color.g = g; 11 | color.b = b; 12 | color.a = a; 13 | return color; 14 | } 15 | 16 | std_msgs::msg::ColorRGBA make(const std::vector& rbga_color) { 17 | return make(rbga_color[0], rbga_color[1], rbga_color[2], rbga_color[3]); 18 | } 19 | 20 | std_msgs::msg::ColorRGBA white(float alpha) { return make(1.0, 1.0, 1.0, alpha); } 21 | 22 | std_msgs::msg::ColorRGBA gray(float alpha) { return make(0.5, 0.5, 0.5, alpha); } 23 | 24 | std_msgs::msg::ColorRGBA red(float alpha) { return make(1.0, 0.0, 0.0, alpha); } 25 | 26 | std_msgs::msg::ColorRGBA green(float alpha) { return make(0.0, 1.0, 0.0, alpha); } 27 | 28 | std_msgs::msg::ColorRGBA blue(float alpha) { return make(0.0, 0.0, 1.0, alpha); } 29 | 30 | std_msgs::msg::ColorRGBA make(const truck_msgs::msg::ControlMode& mode) { 31 | switch (mode.mode) { 32 | case truck_msgs::msg::ControlMode::OFF: 33 | return red(0.5); 34 | case truck_msgs::msg::ControlMode::REMOTE: 35 | return blue(0.5); 36 | case truck_msgs::msg::ControlMode::AUTO: 37 | return green(0.5); 38 | default: 39 | BOOST_ASSERT_MSG(false, "Unknown mode!"); 40 | return white(0); 41 | } 42 | } 43 | 44 | std_msgs::msg::ColorRGBA plasma(float x, float alpha) { 45 | std_msgs::msg::ColorRGBA color; 46 | 47 | color.a = alpha; 48 | 49 | if (x < 0.25) { 50 | color.r = 0; 51 | color.g = 4 * x; 52 | color.b = 1; 53 | } else if (x < 0.5) { 54 | color.r = 0; 55 | color.g = 1; 56 | color.b = 1 + 4 * (0.25 - x); 57 | } else if (x < 0.75) { 58 | color.r = 4 * (x - 0.5); 59 | color.g = 1; 60 | color.b = 0; 61 | } else { 62 | color.r = 1; 63 | color.g = 1 + 4 * (0.75 - x); 64 | color.b = 0; 65 | } 66 | 67 | return color; 68 | } 69 | 70 | } // namespace truck::visualization::color 71 | -------------------------------------------------------------------------------- /packages/visualization/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "visualization/visualization_node.h" 2 | 3 | #include "rclcpp/rclcpp.hpp" 4 | 5 | #include 6 | 7 | int main(int argc, char* argv[]) { 8 | rclcpp::init(argc, argv); 9 | rclcpp::spin(std::make_shared()); 10 | rclcpp::shutdown(); 11 | return 0; 12 | } 13 | -------------------------------------------------------------------------------- /packages/visualization/src/msg.cpp: -------------------------------------------------------------------------------- 1 | #include "visualization/msg.h" 2 | #include "visualization/color.h" 3 | 4 | #include "geom/msg.h" 5 | 6 | namespace truck::visualization { 7 | 8 | namespace msg { 9 | 10 | visualization_msgs::msg::Marker toMarker( 11 | const geom::ComplexPolygon& complex_polygon, std::string frame_id, 12 | std::vector rgba_color, float z_lev) { 13 | visualization_msgs::msg::Marker msg; 14 | msg.header.frame_id = frame_id; 15 | msg.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; 16 | msg.action = visualization_msgs::msg::Marker::ADD; 17 | msg.color = color::make(rgba_color); 18 | msg.pose.position.z = z_lev; 19 | 20 | for (const geom::Triangle& triangle : complex_polygon.triangles()) { 21 | msg.points.push_back(geom::msg::toPoint(triangle.p1)); 22 | msg.points.push_back(geom::msg::toPoint(triangle.p2)); 23 | msg.points.push_back(geom::msg::toPoint(triangle.p3)); 24 | } 25 | 26 | return msg; 27 | } 28 | 29 | } // namespace msg 30 | } // namespace truck::visualization 31 | -------------------------------------------------------------------------------- /packages/waypoint_follower/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(waypoint_follower) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(collision REQUIRED) 6 | find_package(common REQUIRED) 7 | find_package(geom REQUIRED) 8 | find_package(geometry_msgs REQUIRED) 9 | find_package(model REQUIRED) 10 | find_package(motion REQUIRED) 11 | find_package(nav_msgs REQUIRED) 12 | find_package(speed REQUIRED) 13 | find_package(std_srvs REQUIRED) 14 | find_package(rclcpp REQUIRED) 15 | find_package(tf2 REQUIRED) 16 | find_package(tf2_ros REQUIRED) 17 | find_package(truck_msgs REQUIRED) 18 | 19 | add_executable( 20 | ${PROJECT_NAME}_node src/waypoint_follower.cpp src/main.cpp 21 | src/waypoint_follower_node.cpp 22 | ) 23 | 24 | ament_target_dependencies( 25 | ${PROJECT_NAME}_node 26 | collision 27 | common 28 | geom 29 | geometry_msgs 30 | nav_msgs 31 | model 32 | rclcpp 33 | speed 34 | truck_msgs 35 | tf2 36 | tf2_ros 37 | std_srvs 38 | visualization_msgs 39 | ) 40 | 41 | install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME}) 42 | 43 | target_include_directories( 44 | ${PROJECT_NAME}_node 45 | PUBLIC "$" 46 | "$" 47 | "$" 48 | ) 49 | 50 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 51 | 52 | ament_package() 53 | -------------------------------------------------------------------------------- /packages/waypoint_follower/include/waypoint_follower/waypoint_follower.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "common/math.h" 4 | #include "geom/pose.h" 5 | #include "geom/vector.h" 6 | 7 | #include 8 | #include 9 | 10 | namespace truck::waypoint_follower { 11 | 12 | struct Waypoint { 13 | static constexpr uint32_t kNoSeqId = -1; 14 | 15 | Waypoint(uint32_t seq_id, const geom::Vec2& pos); 16 | 17 | uint32_t seq_id; 18 | geom::Vec2 pos; 19 | }; 20 | 21 | struct LinkedPose { 22 | LinkedPose(uint32_t wp_seq_id, const geom::Pose& pose); 23 | 24 | uint32_t wp_seq_id; 25 | geom::Pose pose; 26 | }; 27 | 28 | class WaypointFollower { 29 | public: 30 | struct Parameters { 31 | double resolution{0.05}; 32 | double check_in_distance{0.1}; 33 | double min_distance{0.3}; 34 | }; 35 | 36 | WaypointFollower(const Parameters& params); 37 | 38 | void addEgoWaypoint(const geom::Pose& ego_pose); 39 | bool addWaypoint(const geom::Vec2& waypoint); 40 | void update(const geom::Pose& ego_pose); 41 | void reset(); 42 | 43 | // Last waypoint may be removed explicitly! 44 | bool hasWaypoints() const; 45 | bool isReadyToFinish(const geom::Pose& ego_pose) const; 46 | 47 | const std::deque& path() const; 48 | const std::deque& waypoints() const; 49 | 50 | private: 51 | Parameters params_{}; 52 | 53 | struct State { 54 | uint32_t wp_seq_id; 55 | std::deque path; 56 | std::deque waypoints; 57 | } state_; 58 | }; 59 | 60 | } // namespace truck::waypoint_follower 61 | -------------------------------------------------------------------------------- /packages/waypoint_follower/launch/waypoint_follower.yaml: -------------------------------------------------------------------------------- 1 | launch: 2 | - arg: 3 | name: "model_config" 4 | default: "$(find-pkg-share model)/config/model.yaml" 5 | - arg: 6 | name: "simulation" 7 | default: "false" 8 | - node: 9 | pkg: "waypoint_follower" 10 | exec: "waypoint_follower_node" 11 | name: "waypoint_follower_node" 12 | output: "log" 13 | param: 14 | - { name: "use_sim_time", value: "$(var simulation)" } 15 | - { name: "model_config", value: "$(var model_config)" } 16 | - { name: "period", value: 0.1 } 17 | - { name: "resolution", value: 0.02 } 18 | - { name: "check_in_distance", value: 0.30 } 19 | - { name: "safety_margin", value: 0.15 } 20 | - { name: "distance_to_obstacle", value: 1.5 } 21 | - { name: "acceleration/min", value: -0.5 } 22 | - { name: "acceleration/max", value: 0.3 } 23 | -------------------------------------------------------------------------------- /packages/waypoint_follower/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | waypoint_follower 5 | 0.0.0 6 | waypoint_follower 7 | Simagi Denis 8 | MIT 9 | 10 | ament_cmake 11 | 12 | common 13 | geom 14 | collision 15 | model 16 | motion 17 | nav_msgs 18 | rclcpp 19 | speed 20 | tf2 21 | tf2_ros 22 | truck_interfaces 23 | 24 | ament_cmake_gtest 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /packages/waypoint_follower/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "waypoint_follower/waypoint_follower_node.h" 2 | 3 | #include 4 | 5 | #include 6 | 7 | int main(int argc, char** argv) { 8 | rclcpp::init(argc, argv); 9 | rclcpp::spin(std::make_shared()); 10 | rclcpp::shutdown(); 11 | return 0; 12 | } 13 | -------------------------------------------------------------------------------- /primitives/README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-laboratory/truck/332ff42e67d083e36dcf9d8070f3c9e8527ffc6e/primitives/README.md -------------------------------------------------------------------------------- /primitives/primitives.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "x": 2.0, 4 | "y": 0.0, 5 | "theta": 0.0 6 | }, 7 | { 8 | "x": 2.0, 9 | "y": 1.0, 10 | "theta": 0.7854 11 | }, 12 | { 13 | "x": 2.0, 14 | "y": -1.0, 15 | "theta": -0.7854 16 | }, 17 | { 18 | "x": 1.0, 19 | "y": 1.0, 20 | "theta": 1.5708 21 | }, 22 | { 23 | "x": 1.0, 24 | "y": -1.0, 25 | "theta": -1.5708 26 | }, 27 | { 28 | "x": 0.0, 29 | "y": 2.0, 30 | "theta": 3.1416 31 | }, 32 | { 33 | "x": 0.0, 34 | "y": -2.0, 35 | "theta": -3.1416 36 | } 37 | ] 38 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.ruff] 2 | line-length = 88 3 | target-version = "py311" 4 | exclude = [".cmake-format.py"] 5 | 6 | [tool.ruff.lint] 7 | select = ["I", "F", "E"] 8 | 9 | [tool.ruff.lint.per-file-ignores] 10 | "__init__.py" = ["F401"] 11 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | 2 | -i https://pypi.org/simple 3 | matplotlib==3.5.1 4 | msgpacketizer==0.2.0 5 | numpy==1.23.3 6 | scipy==1.9.3 7 | odrive==0.6.1.post0 8 | pybind11==2.10.0 9 | pyomo==6.1.2 10 | pyserial==3.5 11 | dvc[s3]==3.31.2 12 | pre-commit==3.5.0 13 | importlib-metadata==8.2.0 14 | -------------------------------------------------------------------------------- /scripts/pub.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | . install/setup.bash 3 | 4 | # ros2 topic pub /truck/target truck_interfaces/msg/Point "{x: 5, y: 3, theta: 1.57079632679489661923}" -1 5 | # ros2 topic pub /truck/random_scene truck_interfaces/msg/RandomSeed "{seed: 1, probability: 0.01}" -1 6 | # ros2 topic pub /truck/random_scene truck_interfaces/msg/RandomSeed "{seed: 1, probability: 0.0}" -1 7 | ros2 topic pub /truck/control_command truck_interfaces/msg/Control "{curvature: 1.0, velocity: 1.0, acceleration: 1.0}" -1 8 | -------------------------------------------------------------------------------- /scripts/rosenv.sh: -------------------------------------------------------------------------------- 1 | echo "Loading ROS environment variables..." 2 | source /opt/ros/$ROS_DISTRO/setup.sh 3 | source packages/install/setup.sh 4 | -------------------------------------------------------------------------------- /setup/README.md: -------------------------------------------------------------------------------- 1 | # Jetson Xavier NX (NVME) 2 | 3 | 1. Install [SDK manager](https://developer.nvidia.com/nvidia-sdk-manager). 4 | 2. Set jumper for 9 and 10 pins (under brain board). 5 | 3. Power on board and connect it to host machine via Micro-USB port. 6 | 4. Run SDK manager and follow instructions. 7 | 5. Remove jumper just before flashing process. 8 | 6. After flashing make initial configuration then run fron current dir. 9 | 10 | ```sudo bash jetson.sh``` 11 | 12 | # Jetson Nano (SD card) 13 | 1. Follow this [instructions](https://developer.nvidia.com/embedded/learn/get-started-jetson-nano-devkit). 14 | 2. After flashing make initial configuration then run fron current dir. 15 | 16 | ```sudo bash jetson.sh``` 17 | -------------------------------------------------------------------------------- /setup/jetson.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | DEBIAN_FRONTEND=noninteractive 4 | 5 | # upgrade pkgs 6 | 7 | cp ./misc/apt_preferences /etc/apt/preferences 8 | 9 | apt update -q && apt upgrade -yq 10 | apt install -qy -yq --no-install-recommends \ 11 | cmake \ 12 | gnupg2 \ 13 | git \ 14 | docker \ 15 | make \ 16 | wget \ 17 | curl \ 18 | python3 \ 19 | python3-dev \ 20 | python3-setuptools \ 21 | python3-pip \ 22 | tar \ 23 | vim 24 | 25 | # install jtop 26 | 27 | apt install git cmake 28 | apt install -qy -yq --no-install-recommends \ 29 | libhdf5-serial-dev \ 30 | hdf5-tools \ 31 | libatlas-base-dev \ 32 | gfortran 33 | 34 | python3 -m pip install -U jetson-stats 35 | 36 | # configure docker 37 | 38 | usermod -aG docker ${USER} 39 | 40 | # setup jetpack 41 | 42 | apt install -yq --no-install-recommends nvidia-jetpack 43 | 44 | # realsense 45 | 46 | LIBRS_VERSION="2.50.0" 47 | 48 | cd /tmp 49 | wget -qO - https://github.com/IntelRealSense/librealsense/archive/refs/tags/v${LIBRS_VERSION}.tar.gz | tar -xz\ 50 | cd librealsense-${LIBRS_VERSION} 51 | bash scripts/patch-realsense-ubuntu-L4T.sh 52 | bash scripts/setup_udev_rules.sh 53 | rm -rf /tmp/* 54 | 55 | # clean 56 | apt autoremove 57 | 58 | echo "Please, restart your Jetson!" 59 | -------------------------------------------------------------------------------- /setup/misc/apt_preferences: -------------------------------------------------------------------------------- 1 | Package: docker.io 2 | Pin: version 19.03* 3 | Pin-Priority: 1001 4 | 5 | Package: containerd 6 | Pin: version 1.5.2* 7 | Pin-Priority: 100 8 | --------------------------------------------------------------------------------