├── .github ├── Doxyfile └── workflows │ ├── close-inactive-issues.yml │ ├── cpp-formatter.yml │ ├── create-release.yml │ ├── doxygen-deployment.yml │ ├── humble-docker-build.yml │ ├── humble-docker-push.yml │ ├── iron-docker-build.yml │ ├── iron-docker-push.yml │ └── python-formatter.yml ├── .gitignore ├── CITATION.cff ├── Dockerfile ├── LICENSE ├── README.md ├── docs ├── forest.png ├── mars.png ├── moon.png └── rover.png ├── rover_bringup ├── CMakeLists.txt ├── config │ ├── ublox.yaml │ └── urg_node_serial.yaml ├── launch │ ├── rover.launch.py │ ├── ublox.launch.py │ └── urg_node.launch.py └── package.xml ├── rover_description ├── CMakeLists.txt ├── config │ └── control.yaml ├── launch │ └── robot_state_publisher.launch.py ├── meshes │ ├── bases │ │ ├── back_left_steering_asm.stl │ │ ├── back_right_steering_asm.stl │ │ ├── body_box.stl │ │ ├── differential.stl │ │ ├── front_left_steering_asm.stl │ │ ├── front_right_steering_asm.stl │ │ ├── left_bogie_asm.stl │ │ ├── left_rocker_asm.stl │ │ ├── right_bogie_asm.stl │ │ ├── right_rocker_asm.stl │ │ └── turnbuckle.stl │ ├── sensors │ │ ├── asus_xtion │ │ │ ├── asus_xtion_pro_live.dae │ │ │ └── asus_xtion_pro_live.png │ │ └── hokuyo │ │ │ ├── hokuyo.png │ │ │ ├── hokuyo3d.dae │ │ │ ├── hokuyo3d.jpg │ │ │ ├── hokuyo3d.stl │ │ │ ├── hokuyo_urg_04lx.dae │ │ │ ├── hokuyo_urg_04lx.stl │ │ │ ├── hokuyo_ust_10lx.dae │ │ │ ├── hokuyo_ust_20lx.dae │ │ │ ├── hokuyo_utm_30lx.dae │ │ │ └── hokuyo_utm_30lx.stl │ └── wheels │ │ ├── wheel.stl │ │ └── wheel_asm.stl ├── package.xml ├── robots │ └── rover.urdf.xacro └── urdf │ ├── bases │ ├── body.urdf.xacro │ ├── bogie.urdf.xacro │ ├── differential.urdf.xacro │ ├── rocker.urdf.xacro │ └── turnbuckle.urdf.xacro │ ├── inertials.xacro │ ├── materials.xacro │ ├── sensors │ ├── all_sensors.urdf.xacro │ ├── asus_xtion.urdf.xacro │ ├── hokuyo.urdf.xacro │ └── imu.urdf.xacro │ └── wheels │ ├── corner.urdf.xacro │ └── wheel.urdf.xacro ├── rover_gazebo ├── CMakeLists.txt ├── config │ └── gazebo.yaml ├── include │ └── rover_gazebo │ │ └── motors_command_parser_node.hpp ├── launch │ ├── curiosity.launch.py │ ├── forest.launch.py │ ├── gazebo.launch.py │ ├── include │ │ ├── cmd_vel.launch.py │ │ └── spawn.launch.py │ ├── low_moon.launch.py │ ├── mars.launch.py │ └── moon.launch.py ├── models │ ├── crater │ │ ├── meshes │ │ │ ├── crater4k.stl │ │ │ └── crater_martian_red.dae │ │ ├── model.config │ │ └── model.sdf │ ├── curiosity_path │ │ ├── meshes │ │ │ ├── curiosity_path25k.dae │ │ │ ├── curiosity_path25k.stl │ │ │ └── mars_texture_lq.jpg │ │ ├── model.config │ │ └── model.sdf │ ├── jaggedrock │ │ ├── meshes │ │ │ ├── basecolor.jpg │ │ │ ├── bumpy.jpg │ │ │ └── jaggedrock_texture.dae │ │ ├── model.config │ │ └── model.sdf │ ├── low_moon │ │ ├── model.config │ │ └── model.sdf │ ├── mars_terrain │ │ ├── meshes │ │ │ ├── mars_texture.jpg │ │ │ ├── mars_texture_lq.jpg │ │ │ ├── reddish-sand.jpg │ │ │ ├── terrain.dae │ │ │ ├── terrain.stl │ │ │ └── terrain_textured.dae │ │ ├── model.config │ │ └── model.sdf │ ├── moon │ │ ├── materials │ │ │ ├── heightmap │ │ │ │ ├── moon.png │ │ │ │ └── moon.xcf │ │ │ └── textures │ │ │ │ ├── ground.jpg │ │ │ │ ├── ground_1.jpg │ │ │ │ └── ground_normal.jpg │ │ ├── model.config │ │ └── model.sdf │ ├── rock9 │ │ ├── meshes │ │ │ ├── rock9.dae │ │ │ ├── rock9.stl │ │ │ └── texture.jpg │ │ ├── model.config │ │ └── model.sdf │ ├── rockformation │ │ ├── meshes │ │ │ ├── SMALL_RED_BaseColor.png │ │ │ └── rockformation.dae │ │ ├── model.config │ │ └── model.sdf │ └── terrain │ │ ├── materials │ │ └── textures │ │ │ ├── terrain.png │ │ │ └── terrain.xcf │ │ ├── model.config │ │ └── model.sdf ├── package.xml ├── rviz │ └── default.rviz ├── src │ └── motors_command_parser_node.cpp └── worlds │ ├── curiosity.world │ ├── empty.world │ ├── forest.world │ ├── low_moon.world │ ├── mars.world │ └── moon.world ├── rover_localization ├── CMakeLists.txt ├── config │ └── ekf.yaml ├── launch │ ├── ekf.launch.py │ ├── localization.launch.py │ ├── rgbd_odometry.launch.py │ └── rtabmap.launch.py └── package.xml ├── rover_motor_controller ├── launch │ └── motor_controller.launch.py ├── package.xml ├── resource │ └── rover_motor_controller ├── rover_motor_controller │ ├── __init__.py │ ├── controller_node_main.py │ ├── lx16a │ │ ├── __init__.py │ │ ├── lx16a.py │ │ ├── lx16a_consts.py │ │ └── motor_controller.py │ ├── motor_controller │ │ ├── __init__.py │ │ ├── controller_node.py │ │ └── vel_parser_node.py │ └── vel_parser_node_main.py ├── setup.cfg ├── setup.py └── test │ ├── test_copyright.py │ ├── test_flake8.py │ └── test_pep257.py ├── rover_motor_controller_cpp ├── .clang-format ├── CMakeLists.txt ├── include │ ├── lx16a │ │ ├── lx16a.hpp │ │ ├── lx16a_consts.hpp │ │ ├── motor_controller.hpp │ │ └── serial.hpp │ └── motor_controller │ │ ├── controller_node.hpp │ │ └── vel_parser_node.hpp ├── launch │ └── motor_controller.launch.py ├── package.xml └── src │ ├── controller_node_main.cpp │ ├── lx16a │ ├── lx16a.cpp │ ├── motor_controller.cpp │ └── serial.cpp │ ├── motor_controller │ ├── controller_node.cpp │ └── vel_parser_node.cpp │ └── vel_parser_node_main.cpp ├── rover_msgs ├── CMakeLists.txt ├── msg │ └── MotorsCommand.msg └── package.xml ├── rover_navigation ├── CMakeLists.txt ├── behavior_trees │ └── rover_bt.xml ├── launch │ ├── bringup.launch.py │ └── navigation.launch.py ├── package.xml └── params │ ├── SmacHybrid_RPP.yaml │ ├── SmacHybrid_TEB.yaml │ ├── SmacLattice_RPP.yaml │ └── SmacLattice_TEB.yaml ├── rover_service ├── install.sh ├── rover.service └── rover.sh └── rover_teleop ├── config └── ps3.yaml ├── launch └── joy_teleop.launch.py ├── package.xml ├── resource └── rover_teleop ├── rover_teleop ├── __init__.py └── teleop_keyboard_node.py ├── setup.cfg ├── setup.py └── test ├── test_copyright.py ├── test_flake8.py └── test_pep257.py /.github/workflows/close-inactive-issues.yml: -------------------------------------------------------------------------------- 1 | name: Close Inactive Issues 2 | on: 3 | schedule: 4 | - cron: "30 1 * * *" 5 | 6 | jobs: 7 | close-issues: 8 | runs-on: ubuntu-latest 9 | 10 | permissions: 11 | issues: write 12 | pull-requests: write 13 | 14 | steps: 15 | - uses: actions/stale@v5 16 | with: 17 | days-before-issue-stale: 30 18 | days-before-issue-close: 14 19 | stale-issue-label: "stale" 20 | stale-issue-message: "This issue is stale because it has been open for 30 days with no activity." 21 | close-issue-message: "This issue was closed because it has been inactive for 14 days since being marked as stale." 22 | days-before-pr-stale: -1 23 | days-before-pr-close: -1 24 | repo-token: ${{ secrets.GITHUB_TOKEN }} 25 | -------------------------------------------------------------------------------- /.github/workflows/cpp-formatter.yml: -------------------------------------------------------------------------------- 1 | name: C++ Formatting Check 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | cpp_formatter: 7 | runs-on: ubuntu-latest 8 | steps: 9 | - name: Checkout code 10 | uses: actions/checkout@v4 11 | 12 | - name: Clang Formatter 13 | uses: DoozyX/clang-format-lint-action@v0.18.1 14 | with: 15 | clangFormatVersion: 14 16 | source: "." 17 | -------------------------------------------------------------------------------- /.github/workflows/create-release.yml: -------------------------------------------------------------------------------- 1 | name: Create Release 2 | 3 | on: 4 | push: 5 | branches: [main] 6 | 7 | jobs: 8 | create_release: 9 | if: startsWith(github.event.head_commit.message, 'new version') 10 | 11 | runs-on: ubuntu-latest 12 | 13 | steps: 14 | - name: Checkout code 15 | uses: actions/checkout@v4 16 | with: 17 | fetch-depth: 0 18 | 19 | - name: Extract version from commit message 20 | run: | 21 | if [[ "${{ github.event.head_commit.message }}" =~ new\ version\ ([0-9]+\.[0-9]+\.[0-9]+) ]]; then 22 | echo "version=${BASH_REMATCH[1]}" >> $GITHUB_ENV 23 | else 24 | echo "Commit message does not match 'new version *.*.*' format." 25 | exit 1 26 | fi 27 | 28 | - name: Get previous tag 29 | run: | 30 | previous_tag=$(git describe --tags --abbrev=0 HEAD^) 31 | echo "previous_tag=$previous_tag" >> $GITHUB_ENV 32 | 33 | - name: Generate release notes with commit messages 34 | run: | 35 | commits=$(git log "${{ env.previous_tag }}..HEAD" --oneline) 36 | echo "release_body<> $GITHUB_ENV 37 | echo "### Changelog from version ${{ env.previous_tag }} to ${{ env.version }}:" >> $GITHUB_ENV 38 | echo "$commits" >> $GITHUB_ENV 39 | echo "EOF" >> $GITHUB_ENV 40 | 41 | - name: Create GitHub release 42 | uses: actions/create-release@latest 43 | env: 44 | GITHUB_TOKEN: ${{ secrets.RELEASE_TOKEN }} 45 | with: 46 | tag_name: "${{ env.version }}" 47 | release_name: "${{ env.version }}" 48 | body: "${{ env.release_body }}" 49 | draft: false 50 | prerelease: false 51 | -------------------------------------------------------------------------------- /.github/workflows/doxygen-deployment.yml: -------------------------------------------------------------------------------- 1 | name: Doxygen Deployment 2 | 3 | on: 4 | release: 5 | types: [published] 6 | 7 | jobs: 8 | doxygen_generation: 9 | runs-on: ubuntu-latest 10 | 11 | permissions: 12 | contents: write 13 | 14 | steps: 15 | - name: Checkout code 16 | uses: actions/checkout@v4 17 | with: 18 | fetch-depth: 0 19 | 20 | - name: Generate Doxygen 21 | uses: mattnotmitt/doxygen-action@edge 22 | with: 23 | doxyfile-path: ".github/Doxyfile" 24 | 25 | - name: Create redirect for /latest/ 26 | run: | 27 | mkdir -p redirect 28 | echo '' > redirect/index.html 29 | 30 | - name: Deploy Doxygen page 31 | uses: peaceiris/actions-gh-pages@v4 32 | with: 33 | github_token: ${{ secrets.GITHUB_TOKEN }} 34 | publish_branch: gh-pages 35 | publish_dir: docs/html 36 | destination_dir: ${{ github.event.release.tag_name }} 37 | keep_files: true 38 | 39 | - name: Deploy redirect to /latest/ 40 | uses: peaceiris/actions-gh-pages@v4 41 | with: 42 | github_token: ${{ secrets.GITHUB_TOKEN }} 43 | publish_branch: gh-pages 44 | publish_dir: redirect 45 | destination_dir: latest 46 | -------------------------------------------------------------------------------- /.github/workflows/humble-docker-build.yml: -------------------------------------------------------------------------------- 1 | name: Humble Docker Build 2 | 3 | on: 4 | push: 5 | pull_request: 6 | schedule: 7 | - cron: "0 5 * * 1" 8 | 9 | jobs: 10 | humble_docker_build: 11 | runs-on: ubuntu-latest 12 | steps: 13 | - name: Checkout code 14 | uses: actions/checkout@v4 15 | 16 | - name: Set up Docker Buildx 17 | uses: docker/setup-buildx-action@v3 18 | 19 | - name: Build docker 20 | uses: docker/build-push-action@v6 21 | with: 22 | build-args: ROS_DISTRO=humble 23 | push: false 24 | -------------------------------------------------------------------------------- /.github/workflows/humble-docker-push.yml: -------------------------------------------------------------------------------- 1 | name: Humble Docker Push 2 | 3 | on: 4 | release: 5 | types: [published] 6 | 7 | jobs: 8 | humble_docker_push: 9 | runs-on: ubuntu-latest 10 | steps: 11 | - name: Checkout code 12 | uses: actions/checkout@v4 13 | with: 14 | fetch-depth: 0 15 | 16 | - name: Set up Docker Buildx 17 | uses: docker/setup-buildx-action@v3 18 | 19 | - name: Login to Docker Hub 20 | uses: docker/login-action@v3 21 | with: 22 | username: ${{ secrets.DOCKERHUB_USERNAME }} 23 | password: ${{ secrets.DOCKERHUB_TOKEN }} 24 | 25 | - name: Build and push docker 26 | uses: docker/build-push-action@v6 27 | with: 28 | build-args: ROS_DISTRO=humble 29 | push: true 30 | tags: mgons/rover:humble-${{ github.event.release.tag_name }} 31 | -------------------------------------------------------------------------------- /.github/workflows/iron-docker-build.yml: -------------------------------------------------------------------------------- 1 | name: Iron Docker Build 2 | 3 | on: 4 | push: 5 | pull_request: 6 | schedule: 7 | - cron: "0 5 * * 1" 8 | 9 | jobs: 10 | iron_docker_build: 11 | runs-on: ubuntu-latest 12 | steps: 13 | - name: Checkout code 14 | uses: actions/checkout@v4 15 | 16 | - name: Set up Docker Buildx 17 | uses: docker/setup-buildx-action@v3 18 | 19 | - name: Build docker 20 | uses: docker/build-push-action@v6 21 | with: 22 | build-args: ROS_DISTRO=iron 23 | push: false 24 | -------------------------------------------------------------------------------- /.github/workflows/iron-docker-push.yml: -------------------------------------------------------------------------------- 1 | name: Iron Docker Push 2 | 3 | on: 4 | release: 5 | types: [published] 6 | 7 | jobs: 8 | iron_docker_push: 9 | runs-on: ubuntu-latest 10 | steps: 11 | - name: Checkout code 12 | uses: actions/checkout@v4 13 | with: 14 | fetch-depth: 0 15 | 16 | - name: Get tag name 17 | run: | 18 | tag_name=$(git describe --tags --abbrev=0 HEAD^) 19 | echo "tag_name=$tag_name" >> $GITHUB_ENV 20 | 21 | - name: Set up Docker Buildx 22 | uses: docker/setup-buildx-action@v3 23 | 24 | - name: Login to Docker Hub 25 | uses: docker/login-action@v3 26 | with: 27 | username: ${{ secrets.DOCKERHUB_USERNAME }} 28 | password: ${{ secrets.DOCKERHUB_TOKEN }} 29 | 30 | - name: Build and push docker 31 | uses: docker/build-push-action@v6 32 | with: 33 | build-args: ROS_DISTRO=iron 34 | push: true 35 | tags: mgons/rover:iron-${{ github.event.release.tag_name }} 36 | -------------------------------------------------------------------------------- /.github/workflows/python-formatter.yml: -------------------------------------------------------------------------------- 1 | name: Python Formatting Check 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | python_formatter: 7 | runs-on: ubuntu-latest 8 | steps: 9 | - name: Checkout code 10 | uses: actions/checkout@v4 11 | 12 | - name: Black Formatter 13 | uses: lgeiger/black-action@master 14 | with: 15 | args: ". --check --diff --line-length 90" 16 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | log 3 | __pycache__ -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | message: "If you use this software, please cite it as below." 3 | authors: 4 | - family-names: "González-Santamarta" 5 | given-names: "Miguel Á." 6 | title: "ros2_rover" 7 | date-released: 2021-07-28 8 | url: "https://github.com/mgonzs13/ros2_rover" 9 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | ARG ROS_DISTRO=humble 2 | FROM ros:${ROS_DISTRO} AS deps 3 | 4 | # Create ros2_ws and copy files 5 | WORKDIR /root/ros2_ws 6 | SHELL ["/bin/bash", "-c"] 7 | COPY . /root/ros2_ws/src 8 | 9 | # Install dependencies 10 | RUN apt-get update \ 11 | && apt-get -y --quiet --no-install-recommends install \ 12 | gcc \ 13 | git \ 14 | wget \ 15 | python3 \ 16 | python3-pip 17 | RUN rosdep install --from-paths src --ignore-src -r -y 18 | 19 | # Colcon the ws 20 | FROM deps AS builder 21 | ARG CMAKE_BUILD_TYPE=Release 22 | RUN source /opt/ros/${ROS_DISTRO}/setup.bash && colcon build 23 | 24 | # Source the ROS 2 setup file 25 | RUN echo "source /root/ros2_ws/install/setup.bash" >> ~/.bashrc 26 | 27 | # Run a default command, e.g., starting a bash shell 28 | CMD ["bash"] 29 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Miguel Ángel González Santamarta 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. -------------------------------------------------------------------------------- /docs/forest.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/docs/forest.png -------------------------------------------------------------------------------- /docs/mars.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/docs/mars.png -------------------------------------------------------------------------------- /docs/moon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/docs/moon.png -------------------------------------------------------------------------------- /docs/rover.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/docs/rover.png -------------------------------------------------------------------------------- /rover_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(rover_bringup) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | 7 | # install the launch directory 8 | install(DIRECTORY 9 | launch config 10 | DESTINATION share/${PROJECT_NAME}/ 11 | ) 12 | 13 | ament_package() 14 | -------------------------------------------------------------------------------- /rover_bringup/config/ublox.yaml: -------------------------------------------------------------------------------- 1 | ublox_gps_node: 2 | ros__parameters: 3 | debug: 0 # Range 0-4 (0 means no debug statements will print) 4 | device: /dev/ttyACM0 5 | frame_id: gps 6 | uart1: 7 | baudrate: 9600 8 | 9 | # TMODE3 Config 10 | tmode3: 1 # Survey-In Mode 11 | sv_in: 12 | reset: 13 | True # True: disables and re-enables survey-in (resets) 14 | # False: Disables survey-in only if TMODE3 is disabled 15 | 16 | min_dur: 300 # Survey-In Minimum Duration [s] 17 | acc_lim: 3.0 # Survey-In Accuracy Limit [m] 18 | 19 | inf: 20 | all: false # Whether to display all INF messages in console 21 | 22 | publish: 23 | all: false 24 | aid: 25 | hui: false 26 | nav: 27 | posecef: false 28 | -------------------------------------------------------------------------------- /rover_bringup/config/urg_node_serial.yaml: -------------------------------------------------------------------------------- 1 | urg_node: 2 | ros__parameters: 3 | angle_max: 3.14 4 | angle_min: -3.14 5 | ip_port: 10940 6 | serial_port: "/dev/ttyACM0" 7 | serial_baud: 115200 8 | laser_frame_id: "laser" 9 | calibrate_time: false 10 | default_user_latency: 0.0 11 | diagnostics_tolerance: 0.05 12 | diagnostics_window_time: 5.0 13 | error_limit: 4 14 | get_detailed_status: false 15 | publish_intensity: false 16 | publish_multiecho: false 17 | cluster: 1 18 | skip: 0 19 | -------------------------------------------------------------------------------- /rover_bringup/launch/rover.launch.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | import os 25 | from launch import LaunchDescription 26 | from launch.actions import SetEnvironmentVariable, IncludeLaunchDescription 27 | from launch.launch_description_sources import PythonLaunchDescriptionSource 28 | from ament_index_python.packages import get_package_share_directory 29 | from launch_ros.actions import PushRosNamespace 30 | 31 | 32 | def generate_launch_description(): 33 | rover_bringup_shared_dir = get_package_share_directory("rover_bringup") 34 | rover_motor_controller_shared_dir = get_package_share_directory( 35 | "rover_motor_controller_cpp" 36 | ) 37 | rover_teleop_shared_dir = get_package_share_directory("rover_teleop") 38 | 39 | stdout_linebuf_envvar = SetEnvironmentVariable( 40 | "RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED", "1" 41 | ) 42 | 43 | # 44 | # LAUNCHES 45 | # 46 | 47 | urg_node_action_cmd = IncludeLaunchDescription( 48 | PythonLaunchDescriptionSource( 49 | os.path.join(rover_bringup_shared_dir, "launch", "urg_node.launch.py") 50 | ), 51 | launch_arguments={ 52 | "config_filepath": os.path.join( 53 | rover_bringup_shared_dir, "config", "urg_node_serial.yaml" 54 | ) 55 | }.items(), 56 | ) 57 | 58 | teleop_twist_joy_action_cmd = IncludeLaunchDescription( 59 | PythonLaunchDescriptionSource( 60 | os.path.join(rover_teleop_shared_dir, "launch", "joy_teleop.launch.py") 61 | ) 62 | ) 63 | 64 | rover_motor_controller_action_cmd = IncludeLaunchDescription( 65 | PythonLaunchDescriptionSource( 66 | os.path.join( 67 | rover_motor_controller_shared_dir, "launch", "motor_controller.launch.py" 68 | ) 69 | ) 70 | ) 71 | 72 | ld = LaunchDescription() 73 | 74 | ld.add_action(stdout_linebuf_envvar) 75 | 76 | ld.add_action(urg_node_action_cmd) 77 | ld.add_action(teleop_twist_joy_action_cmd) 78 | ld.add_action(rover_motor_controller_action_cmd) 79 | 80 | return ld 81 | -------------------------------------------------------------------------------- /rover_bringup/launch/ublox.launch.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | import os 25 | import ament_index_python.packages 26 | from launch import LaunchDescription 27 | from launch_ros.actions import Node 28 | from launch.actions import RegisterEventHandler, EmitEvent 29 | from launch.event_handlers import OnProcessExit 30 | from launch.events import Shutdown 31 | 32 | 33 | def generate_launch_description(): 34 | 35 | config_directory = os.path.join( 36 | ament_index_python.packages.get_package_share_directory("rover_bringup"), "config" 37 | ) 38 | params = os.path.join(config_directory, "ublox.yaml") 39 | 40 | ublox_gps_node = Node( 41 | package="ublox_gps", 42 | executable="ublox_gps_node", 43 | output="both", 44 | parameters=[params], 45 | ) 46 | 47 | shutdown_event = RegisterEventHandler( 48 | event_handler=OnProcessExit( 49 | target_action=ublox_gps_node, 50 | on_exit=[EmitEvent(event=Shutdown())], 51 | ) 52 | ) 53 | 54 | ld = LaunchDescription() 55 | ld.add_action(ublox_gps_node) 56 | ld.add_action(shutdown_event) 57 | return ld 58 | -------------------------------------------------------------------------------- /rover_bringup/launch/urg_node.launch.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | import os 25 | from ament_index_python.packages import get_package_share_directory 26 | import launch 27 | import launch_ros.actions 28 | 29 | 30 | def generate_launch_description(): 31 | sensor_interface = launch.substitutions.LaunchConfiguration("sensor_interface") 32 | config_filepath = launch.substitutions.LaunchConfiguration("config_filepath") 33 | 34 | return launch.LaunchDescription( 35 | [ 36 | launch.actions.DeclareLaunchArgument( 37 | "sensor_interface", 38 | default_value="serial", 39 | description="sensor_interface: supported: serial, ethernet", 40 | ), 41 | launch.actions.DeclareLaunchArgument( 42 | "config_filepath", 43 | default_value=[ 44 | launch.substitutions.TextSubstitution( 45 | text=os.path.join( 46 | get_package_share_directory("urg_node"), "launch", "urg_node_" 47 | ) 48 | ), 49 | sensor_interface, 50 | launch.substitutions.TextSubstitution(text=".yaml"), 51 | ], 52 | ), 53 | launch_ros.actions.Node( 54 | package="urg_node", 55 | executable="urg_node_driver", 56 | output="screen", 57 | parameters=[config_filepath], 58 | ), 59 | ] 60 | ) 61 | -------------------------------------------------------------------------------- /rover_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rover_bringup 5 | 1.1.0 6 | Bringup package 7 | Miguel Ángel González Santamarta 8 | MIT 9 | ament_cmake 10 | rover_motor_controller_cpp 11 | joy_linux 12 | teleop_twist_joy 13 | urg_node 14 | 15 | ament_cmake 16 | 17 | -------------------------------------------------------------------------------- /rover_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(rover_description) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | install( 7 | DIRECTORY config launch meshes urdf robots 8 | DESTINATION share/${PROJECT_NAME} 9 | ) 10 | 11 | ament_package() 12 | -------------------------------------------------------------------------------- /rover_description/config/control.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 # Hz 4 | 5 | position_controller: 6 | type: position_controllers/JointGroupPositionController 7 | 8 | velocity_controller: 9 | type: velocity_controllers/JointGroupVelocityController 10 | 11 | joint_state_broadcaster: 12 | type: joint_state_broadcaster/JointStateBroadcaster 13 | 14 | position_controller: 15 | ros__parameters: 16 | joints: 17 | - front_left_corner_joint 18 | - front_right_corner_joint 19 | - back_left_corner_joint 20 | - back_right_corner_joint 21 | interface_name: position 22 | command_interfaces: 23 | - position 24 | state_interfaces: 25 | - position 26 | - velocity 27 | 28 | velocity_controller: 29 | ros__parameters: 30 | joints: 31 | - front_left_wheel_joint 32 | - mid_left_wheel_joint 33 | - back_left_wheel_joint 34 | - front_right_wheel_joint 35 | - mid_right_wheel_joint 36 | - back_right_wheel_joint 37 | command_interfaces: 38 | - velocity 39 | state_interfaces: 40 | - position 41 | - velocity 42 | 43 | joint_state_broadcaster: 44 | ros__parameters: 45 | publish_rate: 100.0 46 | use_sim_time: true 47 | -------------------------------------------------------------------------------- /rover_description/launch/robot_state_publisher.launch.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | import os 25 | import xacro 26 | from ament_index_python import get_package_share_directory 27 | from launch_ros.actions import Node 28 | from launch import LaunchDescription 29 | 30 | 31 | def generate_launch_description(): 32 | 33 | xacro_file = os.path.join( 34 | get_package_share_directory("rover_description"), "robots/rover.urdf.xacro" 35 | ) 36 | 37 | doc = xacro.parse(open(xacro_file)) 38 | xacro.process_doc(doc) 39 | params = {"robot_description": doc.toxml(), "use_sim_time": True} 40 | 41 | robot_state_publisher_cmd = Node( 42 | name="robot_state_publisher", 43 | package="robot_state_publisher", 44 | executable="robot_state_publisher", 45 | output="screen", 46 | parameters=[params], 47 | ) 48 | 49 | ld = LaunchDescription() 50 | 51 | ld.add_action(robot_state_publisher_cmd) 52 | 53 | return ld 54 | -------------------------------------------------------------------------------- /rover_description/meshes/bases/back_left_steering_asm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/bases/back_left_steering_asm.stl -------------------------------------------------------------------------------- /rover_description/meshes/bases/back_right_steering_asm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/bases/back_right_steering_asm.stl -------------------------------------------------------------------------------- /rover_description/meshes/bases/body_box.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/bases/body_box.stl -------------------------------------------------------------------------------- /rover_description/meshes/bases/differential.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/bases/differential.stl -------------------------------------------------------------------------------- /rover_description/meshes/bases/front_left_steering_asm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/bases/front_left_steering_asm.stl -------------------------------------------------------------------------------- /rover_description/meshes/bases/front_right_steering_asm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/bases/front_right_steering_asm.stl -------------------------------------------------------------------------------- /rover_description/meshes/bases/left_bogie_asm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/bases/left_bogie_asm.stl -------------------------------------------------------------------------------- /rover_description/meshes/bases/left_rocker_asm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/bases/left_rocker_asm.stl -------------------------------------------------------------------------------- /rover_description/meshes/bases/right_bogie_asm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/bases/right_bogie_asm.stl -------------------------------------------------------------------------------- /rover_description/meshes/bases/right_rocker_asm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/bases/right_rocker_asm.stl -------------------------------------------------------------------------------- /rover_description/meshes/bases/turnbuckle.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/bases/turnbuckle.stl -------------------------------------------------------------------------------- /rover_description/meshes/sensors/asus_xtion/asus_xtion_pro_live.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/sensors/asus_xtion/asus_xtion_pro_live.png -------------------------------------------------------------------------------- /rover_description/meshes/sensors/hokuyo/hokuyo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/sensors/hokuyo/hokuyo.png -------------------------------------------------------------------------------- /rover_description/meshes/sensors/hokuyo/hokuyo3d.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/sensors/hokuyo/hokuyo3d.jpg -------------------------------------------------------------------------------- /rover_description/meshes/sensors/hokuyo/hokuyo3d.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/sensors/hokuyo/hokuyo3d.stl -------------------------------------------------------------------------------- /rover_description/meshes/sensors/hokuyo/hokuyo_urg_04lx.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/sensors/hokuyo/hokuyo_urg_04lx.stl -------------------------------------------------------------------------------- /rover_description/meshes/sensors/hokuyo/hokuyo_utm_30lx.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/sensors/hokuyo/hokuyo_utm_30lx.stl -------------------------------------------------------------------------------- /rover_description/meshes/wheels/wheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/wheels/wheel.stl -------------------------------------------------------------------------------- /rover_description/meshes/wheels/wheel_asm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_description/meshes/wheels/wheel_asm.stl -------------------------------------------------------------------------------- /rover_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rover_description 5 | 1.1.0 6 | Rover description package 7 | Miguel Ángel González Santamarta 8 | MIT 9 | ament_cmake 10 | xacro 11 | 12 | ament_cmake 13 | 16 | 17 | -------------------------------------------------------------------------------- /rover_description/urdf/bases/body.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 50 | 51 | 52 | Gazebo/White 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /rover_description/urdf/bases/bogie.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 50 | 51 | 52 | Gazebo/White 53 | 54 | 55 | 56 | 57 | 58 | 63 | 64 | 65 | 66 | 67 | true 68 | true 69 | 70 | 71 | -------------------------------------------------------------------------------- /rover_description/urdf/bases/differential.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 50 | 51 | 52 | Gazebo/White 53 | 54 | 55 | 56 | 57 | 62 | 63 | 64 | 65 | 66 | true 67 | true 68 | 69 | 70 | -------------------------------------------------------------------------------- /rover_description/urdf/bases/rocker.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 51 | 52 | 53 | Gazebo/White 54 | 55 | 56 | 57 | 58 | 59 | 64 | 65 | 66 | 67 | 68 | true 69 | true 70 | 71 | 72 | -------------------------------------------------------------------------------- /rover_description/urdf/bases/turnbuckle.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 23 | 24 | 25 | 26 | 27 | 28 | 0.035 ${reflect * 0.17} 0.07 0 0 0 29 | 30 | 0 0 0 0 0 0 31 | 32 | 33 | package://rover_description/meshes/bases/turnbuckle.stl 34 | 35 | 39 | 40 | 41 | 44 | 45 | 46 | 47 | 0 0 0 0 ${math_pi_over_2} 0 48 | 49 | 50 | 0.07 51 | 0.0025 52 | 53 | 54 | 55 | 56 | 0.01 57 | 58 | 0.0001 59 | 0 60 | 0 61 | 0.0001 62 | 0 63 | 0.0001 64 | 65 | 66 | 67 | 68 | -0.035 0 0 0 0 0 69 | diff_brace_link 70 | ${lat_label}_turnbuckle_link 71 | 72 | 73 | 0.035 0 0 0 0 0 74 | ${lat_label}_rocker_link 75 | ${lat_label}_turnbuckle_link 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /rover_description/urdf/inertials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /rover_description/urdf/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /rover_description/urdf/sensors/all_sensors.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /rover_description/urdf/sensors/hokuyo.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | Gazebo/DarkGrey 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 0 0 0 0 0 0 62 | true 63 | 50 64 | 65 | 66 | 67 | 30 68 | 1 69 | ${min_angle} 70 | ${max_angle} 71 | 72 | 73 | 74 | 0.06 75 | 5.0 76 | 0.004359297 77 | 78 | 79 | gaussian 80 | 0.0 81 | 0.01 82 | 83 | 84 | 85 | 86 | ~/out:=scan 87 | 88 | 89 | 90 | best_effort 91 | 92 | 93 | 94 | 95 | sensor_msgs/LaserScan 96 | ${prefix}_link 97 | 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /rover_description/urdf/wheels/corner.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 51 | 52 | 53 | Gazebo/White 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | true 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | true 82 | 83 | 84 | -------------------------------------------------------------------------------- /rover_description/urdf/wheels/wheel.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 49 | 50 | 51 | Gazebo/DarkGray 52 | 1.0 53 | 1.0 54 | 10000000 55 | 1.0 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | true 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | true 85 | 86 | 87 | -------------------------------------------------------------------------------- /rover_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(rover_gazebo) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | find_package(ament_cmake REQUIRED) 19 | find_package(rclcpp REQUIRED) 20 | find_package(rover_msgs REQUIRED) 21 | find_package(std_msgs REQUIRED) 22 | find_package(trajectory_msgs REQUIRED) 23 | 24 | INCLUDE_DIRECTORIES( 25 | include 26 | ) 27 | 28 | add_executable(motors_command_parser_node src/motors_command_parser_node.cpp) 29 | target_link_libraries(motors_command_parser_node) 30 | ament_target_dependencies(motors_command_parser_node 31 | rclcpp 32 | rover_msgs 33 | std_msgs 34 | trajectory_msgs 35 | ) 36 | install(TARGETS 37 | motors_command_parser_node 38 | DESTINATION lib/${PROJECT_NAME} 39 | ) 40 | 41 | install( 42 | DIRECTORY launch worlds models rviz config 43 | DESTINATION share/${PROJECT_NAME} 44 | ) 45 | 46 | ament_package() 47 | -------------------------------------------------------------------------------- /rover_gazebo/config/gazebo.yaml: -------------------------------------------------------------------------------- 1 | gazebo: 2 | ros__parameters: 3 | publish_rate: 100.0 4 | -------------------------------------------------------------------------------- /rover_gazebo/include/rover_gazebo/motors_command_parser_node.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef MOTORS_COMMANDS_NODE_HPP 24 | #define MOTORS_COMMANDS_NODE_HPP 25 | 26 | #include "rclcpp/rclcpp.hpp" 27 | #include "std_msgs/msg/float64_multi_array.hpp" 28 | 29 | #include "rover_msgs/msg/motors_command.hpp" 30 | 31 | class MotorsCommandParserNode : public rclcpp::Node { 32 | public: 33 | MotorsCommandParserNode(); 34 | void callback(const rover_msgs::msg::MotorsCommand::SharedPtr msg); 35 | int clamp(int range_min, int range_max, int value); 36 | float normalize(int range_min, int range_max, int value); 37 | 38 | private: 39 | rclcpp::Subscription::SharedPtr subscription; 40 | rclcpp::Publisher::SharedPtr 41 | velocity_publisher; 42 | rclcpp::Publisher::SharedPtr 43 | position_publisher; 44 | }; 45 | 46 | #endif -------------------------------------------------------------------------------- /rover_gazebo/launch/curiosity.launch.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | import os 25 | from ament_index_python.packages import get_package_share_directory 26 | from launch import LaunchDescription 27 | from launch.actions import IncludeLaunchDescription 28 | from launch.launch_description_sources import PythonLaunchDescriptionSource 29 | 30 | 31 | def generate_launch_description(): 32 | 33 | pkg_path = get_package_share_directory("rover_gazebo") 34 | 35 | gazebo_cmd = IncludeLaunchDescription( 36 | PythonLaunchDescriptionSource( 37 | os.path.join(pkg_path, "launch", "gazebo.launch.py") 38 | ), 39 | launch_arguments={ 40 | "world": os.path.join(pkg_path, "worlds", "curiosity.world"), 41 | "initial_pose_z": "0.0", 42 | }.items(), 43 | ) 44 | 45 | ld = LaunchDescription() 46 | ld.add_action(gazebo_cmd) 47 | 48 | return ld 49 | -------------------------------------------------------------------------------- /rover_gazebo/launch/forest.launch.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | import os 25 | from ament_index_python.packages import get_package_share_directory 26 | from launch import LaunchDescription 27 | from launch.actions import IncludeLaunchDescription 28 | from launch.launch_description_sources import PythonLaunchDescriptionSource 29 | 30 | 31 | def generate_launch_description(): 32 | 33 | pkg_path = get_package_share_directory("rover_gazebo") 34 | 35 | gazebo_cmd = IncludeLaunchDescription( 36 | PythonLaunchDescriptionSource( 37 | os.path.join(pkg_path, "launch", "gazebo.launch.py") 38 | ), 39 | launch_arguments={ 40 | "world": os.path.join(pkg_path, "worlds", "forest.world"), 41 | "initial_pose_x": "-3.0", 42 | "initial_pose_y": "1.4", 43 | "initial_pose_z": "4.2", 44 | }.items(), 45 | ) 46 | 47 | ld = LaunchDescription() 48 | ld.add_action(gazebo_cmd) 49 | 50 | return ld 51 | -------------------------------------------------------------------------------- /rover_gazebo/launch/include/cmd_vel.launch.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | from launch_ros.actions import Node 25 | from launch import LaunchDescription 26 | 27 | 28 | def generate_launch_description(): 29 | 30 | motors_command_parser_node_cmd = Node( 31 | name="motors_command_parser_node", 32 | package="rover_gazebo", 33 | executable="motors_command_parser_node", 34 | output="log", 35 | ) 36 | 37 | vel_parser_node_cmd = Node( 38 | name="vel_parser_node", 39 | package="rover_motor_controller_cpp", 40 | executable="vel_parser_node", 41 | output="log", 42 | ) 43 | 44 | ld = LaunchDescription() 45 | 46 | ld.add_action(motors_command_parser_node_cmd) 47 | ld.add_action(vel_parser_node_cmd) 48 | 49 | return ld 50 | -------------------------------------------------------------------------------- /rover_gazebo/launch/low_moon.launch.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | import os 25 | from ament_index_python.packages import get_package_share_directory 26 | from launch import LaunchDescription 27 | from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument 28 | from launch.launch_description_sources import PythonLaunchDescriptionSource 29 | from launch.substitutions import LaunchConfiguration 30 | 31 | 32 | def generate_launch_description(): 33 | 34 | pkg_path = get_package_share_directory("rover_gazebo") 35 | 36 | nav2_planner = LaunchConfiguration("nav2_planner") 37 | nav2_planner_cmd = DeclareLaunchArgument( 38 | "nav2_planner", 39 | default_value="SmacHybrid", 40 | choices=["SmacHybrid", "SmacLattice"], 41 | description="Nav2 planner (SmacHybrid or SmacLattice)", 42 | ) 43 | 44 | nav2_controller = LaunchConfiguration("nav2_controller") 45 | nav2_controller_cmd = DeclareLaunchArgument( 46 | "nav2_controller", 47 | default_value="RPP", 48 | choices=["RPP", "TEB"], 49 | description="Nav2 controller (RPP or TEB)", 50 | ) 51 | 52 | gazebo_cmd = IncludeLaunchDescription( 53 | PythonLaunchDescriptionSource( 54 | os.path.join(pkg_path, "launch", "gazebo.launch.py") 55 | ), 56 | launch_arguments={ 57 | "world": os.path.join(pkg_path, "worlds", "low_moon.world"), 58 | "initial_pose_z": "1.16", 59 | "nav2_planner": nav2_planner, 60 | "nav2_controller": nav2_controller, 61 | }.items(), 62 | ) 63 | 64 | ld = LaunchDescription() 65 | ld.add_action(nav2_planner_cmd) 66 | ld.add_action(nav2_controller_cmd) 67 | ld.add_action(gazebo_cmd) 68 | 69 | return ld 70 | -------------------------------------------------------------------------------- /rover_gazebo/launch/mars.launch.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | import os 25 | from ament_index_python.packages import get_package_share_directory 26 | from launch import LaunchDescription 27 | from launch.actions import IncludeLaunchDescription 28 | from launch.launch_description_sources import PythonLaunchDescriptionSource 29 | 30 | 31 | def generate_launch_description(): 32 | 33 | pkg_path = get_package_share_directory("rover_gazebo") 34 | 35 | gazebo_cmd = IncludeLaunchDescription( 36 | PythonLaunchDescriptionSource( 37 | os.path.join(pkg_path, "launch", "gazebo.launch.py") 38 | ), 39 | launch_arguments={ 40 | "world": os.path.join(pkg_path, "worlds", "mars.world"), 41 | "initial_pose_z": "-0.45", 42 | }.items(), 43 | ) 44 | 45 | ld = LaunchDescription() 46 | ld.add_action(gazebo_cmd) 47 | 48 | return ld 49 | -------------------------------------------------------------------------------- /rover_gazebo/launch/moon.launch.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | import os 25 | from ament_index_python.packages import get_package_share_directory 26 | from launch import LaunchDescription 27 | from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument 28 | from launch.launch_description_sources import PythonLaunchDescriptionSource 29 | from launch.substitutions import LaunchConfiguration 30 | 31 | 32 | def generate_launch_description(): 33 | 34 | pkg_path = get_package_share_directory("rover_gazebo") 35 | 36 | nav2_planner = LaunchConfiguration("nav2_planner") 37 | nav2_planner_cmd = DeclareLaunchArgument( 38 | "nav2_planner", 39 | default_value="SmacHybrid", 40 | choices=["SmacHybrid", "SmacLattice"], 41 | description="Nav2 planner (SmacHybrid or SmacLattice)", 42 | ) 43 | 44 | nav2_controller = LaunchConfiguration("nav2_controller") 45 | nav2_controller_cmd = DeclareLaunchArgument( 46 | "nav2_controller", 47 | default_value="RPP", 48 | choices=["RPP", "TEB"], 49 | description="Nav2 controller (RPP or TEB)", 50 | ) 51 | 52 | gazebo_cmd = IncludeLaunchDescription( 53 | PythonLaunchDescriptionSource( 54 | os.path.join(pkg_path, "launch", "gazebo.launch.py") 55 | ), 56 | launch_arguments={ 57 | "world": os.path.join(pkg_path, "worlds", "moon.world"), 58 | "initial_pose_z": "2.11", 59 | "nav2_planner": nav2_planner, 60 | "nav2_controller": nav2_controller, 61 | }.items(), 62 | ) 63 | 64 | ld = LaunchDescription() 65 | ld.add_action(nav2_planner_cmd) 66 | ld.add_action(nav2_controller_cmd) 67 | ld.add_action(gazebo_cmd) 68 | 69 | return ld 70 | -------------------------------------------------------------------------------- /rover_gazebo/models/crater/meshes/crater4k.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/crater/meshes/crater4k.stl -------------------------------------------------------------------------------- /rover_gazebo/models/crater/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | crater 4 | 0.1.0 5 | 6 | Chris Burns 7 | burnsca@amazon.com 8 | 9 | model.sdf 10 | 11 | A random crater 12 | 13 | 14 | -------------------------------------------------------------------------------- /rover_gazebo/models/crater/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 6 | 7 | 0.25 8 | 9 | 0.00015 10 | 0.000000 11 | 0.000000 12 | 0.00015 13 | 0.000000 14 | 0.00015 15 | 16 | 17 | 18 | 19 | 20 | model://crater/meshes/crater_martian_red.dae 21 | 1 1 1 22 | 23 | 24 | 25 | 26 | 27 | 30.0 28 | 30.0 29 | 30 | 31 | 32 | 33 | 1000000.0 34 | 100.0 35 | 1.0 36 | 0.002 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | model://crater/meshes/crater_martian_red.dae 45 | 1 1 1 46 | 47 | 48 | 56 | 57 | 58 | 0.000000 59 | 0.000000 60 | 61 | 0 62 | 0 63 | 1 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /rover_gazebo/models/curiosity_path/meshes/curiosity_path25k.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/curiosity_path/meshes/curiosity_path25k.stl -------------------------------------------------------------------------------- /rover_gazebo/models/curiosity_path/meshes/mars_texture_lq.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/curiosity_path/meshes/mars_texture_lq.jpg -------------------------------------------------------------------------------- /rover_gazebo/models/curiosity_path/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | curiosity_path 4 | 0.1.0 5 | 6 | Miguel Angel Rodriguez 7 | duckfrost@theconstructsim.com 8 | 9 | model.sdf 10 | 11 | NASA's most advanced Mars rover Curiosity has landed on the Red Planet.Source 3D Models: https://nasa3d.arc.nasa.gov/detail/curiosity-path 12 | 13 | 14 | -------------------------------------------------------------------------------- /rover_gazebo/models/curiosity_path/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://curiosity_path/meshes/curiosity_path25k.stl 10 | 1 1 1 11 | 12 | 13 | 14 | 15 | 0xffff 16 | 17 | 18 | 19 | 100 20 | 50 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | model://curiosity_path/meshes/curiosity_path25k.dae 29 | 1 1 1 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /rover_gazebo/models/jaggedrock/meshes/basecolor.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/jaggedrock/meshes/basecolor.jpg -------------------------------------------------------------------------------- /rover_gazebo/models/jaggedrock/meshes/bumpy.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/jaggedrock/meshes/bumpy.jpg -------------------------------------------------------------------------------- /rover_gazebo/models/jaggedrock/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | jaggedrock 4 | 0.1.0 5 | 6 | Chris Burns 7 | burnsca@amazon.com 8 | 9 | model.sdf 10 | 11 | random rock 12 | 13 | -------------------------------------------------------------------------------- /rover_gazebo/models/jaggedrock/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 1 8 | 9 | 0.166667 10 | 0 11 | 0 12 | 0.166667 13 | 0 14 | 0.166667 15 | 16 | 0 0 0 0 -0 0 17 | 18 | 19 | 1 20 | 0 21 | 0 22 | 23 | 24 | 25 | 26 | model://jaggedrock/meshes/jaggedrock_texture.dae 27 | 28 | 29 | 30 | 31 | 32 | 1 33 | 1 34 | 0 0 0 35 | 0 36 | 0 37 | 38 | 39 | 1 40 | 0 41 | 0 42 | 1 43 | 44 | 0 45 | 46 | 47 | 48 | 49 | 0 50 | 1e+06 51 | 52 | 53 | 0 54 | 1 55 | 1 56 | 57 | 0 58 | 0.2 59 | 1e+13 60 | 1 61 | 0.01 62 | 0 63 | 64 | 65 | 1 66 | -0.01 67 | 0 68 | 0.2 69 | 1e+13 70 | 1 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | model://jaggedrock/meshes/jaggedrock_texture.dae 80 | 81 | 82 | 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /rover_gazebo/models/low_moon/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | low_moon 5 | 1.0 6 | model.sdf 7 | 8 | Miguel A. G. Santamarta 9 | mgons@unileon.es 10 | 11 | 12 | 13 | Low Moon 14 | 15 | 16 | -------------------------------------------------------------------------------- /rover_gazebo/models/low_moon/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://moon/materials/heightmap/moon.png 10 | 100 100 3 11 | 0 0 0 12 | 13 | 14 | 15 | 16 | 17 | 100 18 | 50 19 | 20 | 21 | 22 | 23 | 24 | true 25 | 26 | 27 | 28 | model://moon/materials/textures/ground_1.jpg 29 | model://moon/materials/textures/ground_normal.jpg 30 | 1 31 | 32 | model://moon/materials/heightmap/moon.png 33 | 100 100 3 34 | 0 0 0 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /rover_gazebo/models/mars_terrain/meshes/mars_texture.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/mars_terrain/meshes/mars_texture.jpg -------------------------------------------------------------------------------- /rover_gazebo/models/mars_terrain/meshes/mars_texture_lq.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/mars_terrain/meshes/mars_texture_lq.jpg -------------------------------------------------------------------------------- /rover_gazebo/models/mars_terrain/meshes/reddish-sand.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/mars_terrain/meshes/reddish-sand.jpg -------------------------------------------------------------------------------- /rover_gazebo/models/mars_terrain/meshes/terrain.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/mars_terrain/meshes/terrain.stl -------------------------------------------------------------------------------- /rover_gazebo/models/mars_terrain/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | mars_terrain 4 | 0.1.0 5 | 6 | Chris Burns 7 | burnsca@amazon.com 8 | 9 | model.sdf 10 | 11 | Martian Terrain 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /rover_gazebo/models/mars_terrain/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 6 | 7 | 8 | 9 | model://mars_terrain/meshes/terrain_textured.dae 10 | 11 | 12 | 13 | 14 | 15 | 100 16 | 50 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | model://mars_terrain/meshes/terrain_textured.dae 25 | 1 1 1 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /rover_gazebo/models/moon/materials/heightmap/moon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/moon/materials/heightmap/moon.png -------------------------------------------------------------------------------- /rover_gazebo/models/moon/materials/heightmap/moon.xcf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/moon/materials/heightmap/moon.xcf -------------------------------------------------------------------------------- /rover_gazebo/models/moon/materials/textures/ground.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/moon/materials/textures/ground.jpg -------------------------------------------------------------------------------- /rover_gazebo/models/moon/materials/textures/ground_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/moon/materials/textures/ground_1.jpg -------------------------------------------------------------------------------- /rover_gazebo/models/moon/materials/textures/ground_normal.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/moon/materials/textures/ground_normal.jpg -------------------------------------------------------------------------------- /rover_gazebo/models/moon/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | moon 5 | 1.0 6 | model.sdf 7 | 8 | Miguel A. G. Santamarta 9 | mgons@unileon.es 10 | 11 | 12 | 13 | Moon 14 | 15 | 16 | -------------------------------------------------------------------------------- /rover_gazebo/models/moon/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://moon/materials/heightmap/moon.png 10 | 100 100 6 11 | 0 0 0 12 | 13 | 14 | 15 | 16 | 17 | 100 18 | 50 19 | 20 | 21 | 22 | 23 | 24 | true 25 | 26 | 27 | 28 | model://moon/materials/textures/ground_1.jpg 29 | model://moon/materials/textures/ground_normal.jpg 30 | 1 31 | 32 | model://moon/materials/heightmap/moon.png 33 | 100 100 6 34 | 0 0 0 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /rover_gazebo/models/rock9/meshes/rock9.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/rock9/meshes/rock9.stl -------------------------------------------------------------------------------- /rover_gazebo/models/rock9/meshes/texture.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/rock9/meshes/texture.jpg -------------------------------------------------------------------------------- /rover_gazebo/models/rock9/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | rock9 4 | 0.1.0 5 | 6 | Chris Burns 7 | burnsca@amazon.com 8 | 9 | model.sdf 10 | 11 | Large single rock 12 | 13 | -------------------------------------------------------------------------------- /rover_gazebo/models/rock9/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 1 8 | 9 | 0.166667 10 | 0 11 | 0 12 | 0.166667 13 | 0 14 | 0.166667 15 | 16 | 0 0 0 0 -0 0 17 | 18 | 19 | 1 20 | 0 21 | 1 22 | 23 | 24 | 25 | 26 | model://rock9/meshes/rock9.dae 27 | 28 | 29 | 30 | 31 | 32 | 1 33 | 1 34 | 0 0 0 35 | 0 36 | 0 37 | 38 | 39 | 1 40 | 0 41 | 0 42 | 1 43 | 44 | 0 45 | 46 | 47 | 48 | 49 | 0 50 | 1e+06 51 | 52 | 53 | 0 54 | 1 55 | 1 56 | 57 | 0 58 | 0.2 59 | 1e+13 60 | 1 61 | 0.01 62 | 0 63 | 64 | 65 | 1 66 | -0.01 67 | 0 68 | 0.2 69 | 1e+13 70 | 1 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | model://rock9/meshes/rock9.dae 80 | 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /rover_gazebo/models/rockformation/meshes/SMALL_RED_BaseColor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/rockformation/meshes/SMALL_RED_BaseColor.png -------------------------------------------------------------------------------- /rover_gazebo/models/rockformation/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | rockformation 4 | 0.1.0 5 | 6 | Chris Burns 7 | burnsca@amazon.com 8 | 9 | model.sdf 10 | 11 | random rock formation 12 | 13 | -------------------------------------------------------------------------------- /rover_gazebo/models/rockformation/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 1 8 | 9 | 0.166667 10 | 0 11 | 0 12 | 0.166667 13 | 0 14 | 0.166667 15 | 16 | 0 0 0 0 -0 0 17 | 18 | 19 | 1 20 | 0 21 | 0 22 | 23 | 24 | 25 | 26 | model://rockformation/meshes/rockformation.dae 27 | 28 | 29 | 30 | 31 | 32 | 1 33 | 1 34 | 0 0 0 35 | 0 36 | 0 37 | 38 | 39 | 1 40 | 0 41 | 0 42 | 1 43 | 44 | 0 45 | 46 | 47 | 48 | 49 | 0 50 | 1e+06 51 | 52 | 53 | 0 54 | 1 55 | 1 56 | 57 | 0 58 | 0.2 59 | 1e+13 60 | 1 61 | 0.01 62 | 0 63 | 64 | 65 | 1 66 | -0.01 67 | 0 68 | 0.2 69 | 1e+13 70 | 1 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | model://rockformation/meshes/rockformation.dae 80 | 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /rover_gazebo/models/terrain/materials/textures/terrain.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/terrain/materials/textures/terrain.png -------------------------------------------------------------------------------- /rover_gazebo/models/terrain/materials/textures/terrain.xcf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_gazebo/models/terrain/materials/textures/terrain.xcf -------------------------------------------------------------------------------- /rover_gazebo/models/terrain/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | terrain 5 | 1.0 6 | model.sdf 7 | 8 | Miguel A. G. Santamarta 9 | mgons@unileon.es 10 | 11 | 12 | 13 | Heigh map terrain 14 | 15 | 16 | -------------------------------------------------------------------------------- /rover_gazebo/models/terrain/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://terrain/materials/textures/terrain.png 10 | 100 100 7 11 | 0 0 0 12 | 13 | 14 | 15 | 16 | 17 | 18 | false 19 | 20 | file://media/materials/textures/dirt_diffusespecular.png 21 | file://media/materials/textures/flat_normal.png 22 | 1 23 | 24 | 25 | file://media/materials/textures/grass_diffusespecular.png 26 | file://media/materials/textures/flat_normal.png 27 | 1 28 | 29 | 30 | file://media/materials/textures/fungus_diffusespecular.png 31 | file://media/materials/textures/flat_normal.png 32 | 1 33 | 34 | 35 | 2 36 | 5 37 | 38 | 39 | 4 40 | 5 41 | 42 | model://terrain/materials/textures/terrain.png 43 | 100 100 7 44 | 0 0 0 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /rover_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rover_gazebo 5 | 1.1.0 6 | Rover Gazebo package 7 | Miguel Ángel González Santamarta 8 | MIT 9 | ament_cmake 10 | gazebo 11 | gazebo_ros2_control 12 | gazebo_ros_pkgs 13 | gazebo_plugins 14 | joint_state_broadcaster 15 | robot_state_publisher 16 | position_controllers 17 | velocity_controllers 18 | controller_manager 19 | rviz2 20 | rover_description 21 | rover_msgs 22 | rover_motor_controller_cpp 23 | std_msgs 24 | trajectory_msgs 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /rover_gazebo/src/motors_command_parser_node.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #define BOOST_BIND_NO_PLACEHOLDERS 24 | 25 | #include 26 | 27 | #include "rclcpp/rclcpp.hpp" 28 | #include "std_msgs/msg/float64_multi_array.hpp" 29 | 30 | #include "rover_gazebo/motors_command_parser_node.hpp" 31 | #include "rover_msgs/msg/motors_command.hpp" 32 | 33 | using std::placeholders::_1; 34 | 35 | MotorsCommandParserNode::MotorsCommandParserNode() 36 | : rclcpp::Node("motors_command_parser_node") { 37 | 38 | this->velocity_publisher = 39 | this->create_publisher( 40 | "velocity_controller/commands", 10); 41 | 42 | this->position_publisher = 43 | this->create_publisher( 44 | "position_controller/commands", 10); 45 | 46 | this->subscription = 47 | this->create_subscription( 48 | "motors_command", 10, 49 | std::bind(&MotorsCommandParserNode::callback, this, _1)); 50 | } 51 | 52 | void MotorsCommandParserNode::callback( 53 | const rover_msgs::msg::MotorsCommand::SharedPtr msg) { 54 | 55 | // RCLCPP_INFO(this->get_logger(), 56 | // "Speed: %d %d %d %d %d %d, Sterring: %d, %d, %d, %d", 57 | // msg->drive_motor[0], msg->drive_motor[1], msg->drive_motor[2], 58 | // msg->drive_motor[3], msg->drive_motor[4], msg->drive_motor[5], 59 | // msg->corner_motor[0], msg->corner_motor[1], 60 | // msg->corner_motor[2], msg->corner_motor[3]); 61 | 62 | // create msgs 63 | auto corners_positions = std_msgs::msg::Float64MultiArray(); 64 | auto wheel_velocities = std_msgs::msg::Float64MultiArray(); 65 | 66 | float pi = atan(1) * 4; 67 | 68 | for (long unsigned int i = 0; i < msg->drive_motor.size(); i++) { 69 | 70 | float value = 71 | this->normalize(-1000, 1000, 72 | this->clamp(-1000, 1000, msg->drive_motor[i])) * 73 | 20; 74 | 75 | if (i > 2) { 76 | value *= -1; 77 | } 78 | 79 | wheel_velocities.data.push_back(value); 80 | } 81 | 82 | for (int position : msg->corner_motor) { 83 | 84 | corners_positions.data.push_back( 85 | this->normalize(250, 750, this->clamp(-250, 750, position)) * -pi / 2); 86 | } 87 | 88 | // publish 89 | this->position_publisher->publish(corners_positions); 90 | this->velocity_publisher->publish(wheel_velocities); 91 | } 92 | 93 | int MotorsCommandParserNode::clamp(int range_min, int range_max, int value) { 94 | return std::min(range_max, std::max(range_min, value)); 95 | } 96 | 97 | float MotorsCommandParserNode::normalize(int range_min, int range_max, 98 | int value) { 99 | return 2 * ((float)value - range_min) / (range_max - range_min) - 1; 100 | } 101 | 102 | int main(int argc, char *argv[]) { 103 | rclcpp::init(argc, argv); 104 | rclcpp::spin(std::make_shared()); 105 | rclcpp::shutdown(); 106 | return 0; 107 | } 108 | -------------------------------------------------------------------------------- /rover_gazebo/worlds/curiosity.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | quick 9 | 10 | 11 | 12 | 0.001 13 | 0.02 14 | 100.0 15 | 0.001 16 | 17 | 18 | 0.001 19 | 1 20 | 1000 21 | 0 0 -3.711 22 | 23 | 24 | 25 | model://sun 26 | 27 | 28 | 29 | 202 150 66 30 | 31 | 32 | 33 | model://curiosity_path 34 | curiosity_path 35 | 0 0 8.5 0 0 0 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /rover_gazebo/worlds/empty.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://ground_plane 7 | 8 | 9 | 10 | model://sun 11 | 12 | 13 | 14 | false 15 | 16 | 17 | 18 | 0.001 19 | 1 20 | 1000 21 | 0 0 -9.8 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /rover_gazebo/worlds/low_moon.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 1.0 7 | 8 | 9 | 10 | true 11 | 200 0 14 0 0 0 12 | 0.7 0.7 0.7 1 13 | 0.001 0.001 0.001 1 14 | -1 -0.1 -0.15 15 | 16 | 1000 17 | 1 18 | 0 19 | 20 | 21 | 22 | 23 | 0.30 0.30 0.30 1.0 24 | 0.0 0.0 0.0 1 25 | 0 26 | false 27 | true 28 | 29 | 30 | 31 | 0.001 32 | 0 0 -1.62 33 | 6e-06 2.3e-05 -4.2e-05 34 | 35 | 36 | 37 | 38 | model://low_moon 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /rover_gazebo/worlds/mars.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 202 150 66 6 | 7 | 8 | 9 | 0.001 10 | 0 0 -3.711 11 | 12 | 13 | 14 | 0 20 20 0.1 0.1 0 15 | 1 1 1 1 16 | 1 1 1 1 17 | 18 | 300 19 | 20 | 0.1 0.1 -1 21 | false 22 | 23 | 24 | 25 | model://sun 26 | 27 | 28 | 29 | model://curiosity_path 30 | curiosity_path 31 | -136.5 127 14.21 0 0 0 32 | 33 | 34 | 35 | model://mars_terrain 36 | terrain 37 | 2 1 -.75 0 0 -1.575 38 | 39 | 40 | 41 | 42 | model://mars_terrain 43 | terrain1 44 | -22.869 -11.08 -.75 45 | 46 | 47 | 48 | model://mars_terrain 49 | terrain2 50 | -21 13.09 -.70 .01 0 0 51 | 52 | 53 | 54 | 55 | model://rockformation 56 | rockformation 57 | 1 -9 -0.65 0 0 1.75 58 | 59 | 60 | 61 | model://rockformation 62 | rockformation2 63 | -1 4 -0.5 3.141593 -0.008 1.7915 64 | 65 | 66 | 67 | model://rockformation 68 | rockformation3 69 | 10.401 4.11 -0.65 0 0 0 70 | 71 | 72 | 73 | model://rockformation 74 | rockformation4 75 | -18.77 -9.72 -0.75 0 0 0 76 | 77 | 78 | 79 | model://rockformation 80 | rockformation5 81 | -29.2 -2.79 -0.6 0 0 0 82 | 83 | 84 | 85 | model://rockformation 86 | rockformation7 87 | -17.39 2.28 -0.9 0 0 -0.2 88 | 89 | 90 | 91 | model://jaggedrock 92 | jaggedrock 93 | 5 7 -0.75 0 0 -0.7 94 | 95 | 96 | 97 | model://jaggedrock 98 | jaggedrock2 99 | -9 1 -1 -0.2499 0 -2.399 100 | 101 | 102 | 103 | model://jaggedrock 104 | jaggedrock3 105 | -33.515 -5 -.85 0 0 2.4 106 | 107 | 108 | 109 | 110 | model://rock9 111 | rock9 112 | -4.844 19.6 0 1.05 .6 0 113 | 114 | 115 | 116 | model://rock9 117 | rock9_1 118 | 1.849 23.56 0 0 0 0 119 | 120 | 121 | 122 | model://rock9 123 | rock9_2 124 | -39.766 -19.65 -0.4 0 0 0 125 | 126 | 127 | 128 | model://rock9 129 | rock9_3 130 | -38.024 6.469 -2 0 0 -1.7 131 | 132 | 133 | 134 | model://rock9 135 | rock9_4 136 | -8.1 -8.18 -2.85 0 0 .85 137 | 138 | 139 | 140 | model://crater 141 | crater 142 | -48.502 1.5 -2.1 0 0 0 143 | 144 | 145 | 146 | -------------------------------------------------------------------------------- /rover_gazebo/worlds/moon.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 1.0 7 | 8 | 9 | 10 | true 11 | 200 0 14 0 0 0 12 | 0.7 0.7 0.7 1 13 | 0.001 0.001 0.001 1 14 | -1 -0.1 -0.15 15 | 16 | 1000 17 | 1 18 | 0 19 | 20 | 21 | 22 | 23 | 0.30 0.30 0.30 1.0 24 | 0.0 0.0 0.0 1 25 | 0 26 | false 27 | true 28 | 29 | 30 | 31 | 0.001 32 | 0 0 -1.62 33 | 6e-06 2.3e-05 -4.2e-05 34 | 35 | 36 | 37 | 38 | model://moon 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /rover_localization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(rover_localization) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | 7 | # install the launch directory 8 | install(DIRECTORY 9 | launch config 10 | DESTINATION share/${PROJECT_NAME}/ 11 | ) 12 | 13 | ament_package() 14 | -------------------------------------------------------------------------------- /rover_localization/launch/ekf.launch.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | from launch import LaunchDescription 25 | from ament_index_python.packages import get_package_share_directory 26 | from launch_ros.actions import Node 27 | from launch.substitutions import LaunchConfiguration 28 | from launch.actions import DeclareLaunchArgument 29 | from nav2_common.launch import RewrittenYaml 30 | import os 31 | 32 | 33 | def generate_launch_description(): 34 | 35 | use_sim_time = LaunchConfiguration("use_sim_time") 36 | use_sim_time_cmd = DeclareLaunchArgument( 37 | "use_sim_time", 38 | default_value="False", 39 | description="Use simulation (Gazebo) clock if True", 40 | ) 41 | 42 | params_file = os.path.join( 43 | get_package_share_directory("rover_localization"), "config", "ekf.yaml" 44 | ) 45 | 46 | param_substitutions = {"use_sim_time": use_sim_time} 47 | 48 | configured_params = RewrittenYaml( 49 | source_file=params_file, param_rewrites=param_substitutions, convert_types=True 50 | ) 51 | 52 | ekf_cmd = Node( 53 | package="robot_localization", 54 | executable="ekf_node", 55 | name="ekf_filter_node", 56 | output="log", 57 | parameters=[configured_params], 58 | remappings=[("odometry/filtered", "/odom"), ("accel/filtered", "/accel")], 59 | ) 60 | 61 | ld = LaunchDescription() 62 | 63 | ld.add_action(use_sim_time_cmd) 64 | 65 | ld.add_action(ekf_cmd) 66 | 67 | return ld 68 | -------------------------------------------------------------------------------- /rover_localization/launch/localization.launch.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | import os 25 | from ament_index_python.packages import get_package_share_directory 26 | from launch import LaunchDescription 27 | from launch.actions import IncludeLaunchDescription 28 | from launch.launch_description_sources import PythonLaunchDescriptionSource 29 | from launch.substitutions import LaunchConfiguration 30 | from launch.actions import DeclareLaunchArgument 31 | 32 | 33 | def generate_launch_description(): 34 | 35 | pkg_rover_localization = get_package_share_directory("rover_localization") 36 | 37 | use_sim_time = LaunchConfiguration("use_sim_time") 38 | use_sim_time_cmd = DeclareLaunchArgument( 39 | "use_sim_time", 40 | default_value="False", 41 | description="Use simulation (Gazebo) clock if True", 42 | ) 43 | 44 | rgbd_odometry_cmd = IncludeLaunchDescription( 45 | PythonLaunchDescriptionSource( 46 | os.path.join(pkg_rover_localization, "launch", "rgbd_odometry.launch.py") 47 | ) 48 | ) 49 | 50 | rtabmap_cmd = IncludeLaunchDescription( 51 | PythonLaunchDescriptionSource( 52 | os.path.join(pkg_rover_localization, "launch", "rtabmap.launch.py") 53 | ), 54 | launch_arguments={"use_sim_time": use_sim_time}.items(), 55 | ) 56 | 57 | ekf_cmd = IncludeLaunchDescription( 58 | PythonLaunchDescriptionSource( 59 | os.path.join(pkg_rover_localization, "launch", "ekf.launch.py") 60 | ), 61 | launch_arguments={"use_sim_time": use_sim_time}.items(), 62 | ) 63 | 64 | ld = LaunchDescription() 65 | 66 | ld.add_action(use_sim_time_cmd) 67 | 68 | ld.add_action(rgbd_odometry_cmd) 69 | ld.add_action(rtabmap_cmd) 70 | ld.add_action(ekf_cmd) 71 | 72 | return ld 73 | -------------------------------------------------------------------------------- /rover_localization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rover_localization 5 | 1.1.0 6 | Localization package 7 | Miguel Ángel González Santamarta 8 | MIT 9 | ament_cmake 10 | rtabmap 11 | rtabmap_ros 12 | robot_localization 13 | 14 | ament_cmake 15 | 16 | -------------------------------------------------------------------------------- /rover_motor_controller/launch/motor_controller.launch.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | from launch import LaunchDescription 25 | from launch_ros.actions import Node 26 | from launch.substitutions import LaunchConfiguration 27 | from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable 28 | 29 | 30 | def generate_launch_description(): 31 | 32 | pkg_name = "rover_motor_controller" 33 | stdout_linebuf_envvar = SetEnvironmentVariable( 34 | "RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED", "1" 35 | ) 36 | 37 | # 38 | # ARGS 39 | # 40 | hardware_distances = LaunchConfiguration("hardware_distances") 41 | declare_hardware_distances_cmd = DeclareLaunchArgument( 42 | "hardware_distances", 43 | default_value="[23.0, 25.5, 28.5, 26.0]", 44 | description="Rover hardware distances", 45 | ) 46 | 47 | enc_min = LaunchConfiguration("enc_min") 48 | declare_enc_min_cmd = DeclareLaunchArgument( 49 | "enc_min", 50 | default_value="250", 51 | description="enc_min", 52 | ) 53 | 54 | speed_factor = LaunchConfiguration("speed_factor") 55 | declare_speed_factor_cmd = DeclareLaunchArgument( 56 | "speed_factor", 57 | default_value="10", 58 | description="Speed [-100, +100] * 10 = [-1000, +1000]", 59 | ) 60 | 61 | enc_max = LaunchConfiguration("enc_max") 62 | declare_enc_max_cmd = DeclareLaunchArgument( 63 | "enc_max", 64 | default_value="750", 65 | description="enc_max", 66 | ) 67 | 68 | motor_controller_device = LaunchConfiguration("motor_controller_device") 69 | declare_motor_controller_device_cmd = DeclareLaunchArgument( 70 | "motor_controller_device", 71 | default_value="/dev/ttyUSB0", 72 | description="Motor controller device", 73 | ) 74 | 75 | baud_rate = LaunchConfiguration("baud_rate") 76 | declare_baud_rate_cmd = DeclareLaunchArgument( 77 | "baud_rate", 78 | default_value="115200", 79 | description="baud rate", 80 | ) 81 | 82 | # 83 | # NODES 84 | # 85 | 86 | vel_parser_node_cmd = Node( 87 | package=pkg_name, 88 | executable="vel_parser_node", 89 | name="vel_parser_node", 90 | parameters=[ 91 | { 92 | "hardware_distances": hardware_distances, 93 | "enc_min": enc_min, 94 | "enc_max": enc_max, 95 | "speed_factor": speed_factor, 96 | } 97 | ], 98 | ) 99 | 100 | controller_node_cmd = Node( 101 | package=pkg_name, 102 | executable="controller_node", 103 | name="controller_node", 104 | parameters=[ 105 | {"motor_controller_device": motor_controller_device, "baud_rate": baud_rate} 106 | ], 107 | ) 108 | 109 | ld = LaunchDescription() 110 | 111 | ld.add_action(stdout_linebuf_envvar) 112 | 113 | ld.add_action(declare_hardware_distances_cmd) 114 | ld.add_action(declare_enc_min_cmd) 115 | ld.add_action(declare_enc_max_cmd) 116 | ld.add_action(declare_speed_factor_cmd) 117 | ld.add_action(declare_motor_controller_device_cmd) 118 | ld.add_action(declare_baud_rate_cmd) 119 | 120 | ld.add_action(vel_parser_node_cmd) 121 | ld.add_action(controller_node_cmd) 122 | 123 | return ld 124 | -------------------------------------------------------------------------------- /rover_motor_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rover_motor_controller 5 | 1.1.0 6 | Rover motor controller package (Python) 7 | Miguel Ángel González Santamarta 8 | MIT 9 | ament_copyright 10 | ament_flake8 11 | ament_pep257 12 | python3-pytest 13 | rover_msgs 14 | geometry_msgs 15 | 16 | ament_python 17 | 18 | -------------------------------------------------------------------------------- /rover_motor_controller/resource/rover_motor_controller: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_motor_controller/resource/rover_motor_controller -------------------------------------------------------------------------------- /rover_motor_controller/rover_motor_controller/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_motor_controller/rover_motor_controller/__init__.py -------------------------------------------------------------------------------- /rover_motor_controller/rover_motor_controller/controller_node_main.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | import rclpy 25 | from rover_motor_controller.motor_controller import ControllerNode 26 | 27 | 28 | def main(args=None): 29 | rclpy.init(args=args) 30 | 31 | node = ControllerNode() 32 | 33 | rclpy.spin(node) 34 | 35 | rclpy.shutdown() 36 | 37 | 38 | if __name__ == "__main__": 39 | main() 40 | -------------------------------------------------------------------------------- /rover_motor_controller/rover_motor_controller/lx16a/__init__.py: -------------------------------------------------------------------------------- 1 | from .motor_controller import MotorController 2 | -------------------------------------------------------------------------------- /rover_motor_controller/rover_motor_controller/lx16a/lx16a_consts.py: -------------------------------------------------------------------------------- 1 | SERVO_ID_ALL = 0xFE 2 | SERVO_FRAME_HEADER = 0x55 3 | SERVO_FRAME_HEADER_STRING = "0x55" 4 | 5 | # CMDS 6 | SERVO_MOVE_TIME_WRITE = 1 7 | SERVO_MOVE_TIME_READ = 2 8 | SERVO_MOVE_TIME_WAIT_WRITE = 7 9 | SERVO_MOVE_TIME_WAIT_READ = 8 10 | SERVO_MOVE_START = 11 11 | SERVO_MOVE_STOP = 12 12 | SERVO_ID_WRITE = 13 13 | SERVO_ID_READ = 14 14 | SERVO_ANGLE_OFFSET_ADJUST = 17 15 | SERVO_ANGLE_OFFSET_WRITE = 18 16 | SERVO_ANGLE_OFFSET_READ = 19 17 | SERVO_ANGLE_LIMIT_WRITE = 20 18 | SERVO_ANGLE_LIMIT_READ = 21 19 | SERVO_VIN_LIMIT_WRITE = 22 20 | SERVO_VIN_LIMIT_READ = 23 21 | SERVO_TEMP_MAX_LIMIT_WRITE = 24 22 | SERVO_TEMP_MAX_LIMIT_READ = 25 23 | SERVO_TEMP_READ = 26 24 | SERVO_VIN_READ = 27 25 | SERVO_POS_READ = 28 26 | SERVO_OR_MOTOR_MODE_WRITE = 29 27 | SERVO_OR_MOTOR_MODE_READ = 30 28 | SERVO_LOAD_OR_UNLOAD_WRITE = 31 29 | SERVO_LOAD_OR_UNLOAD_READ = 32 30 | SERVO_LED_CTRL_WRITE = 33 31 | SERVO_LED_CTRL_READ = 34 32 | SERVO_LED_ERROR_WRITE = 35 33 | SERVO_LED_ERROR_READ = 36 34 | 35 | # ERRORS 36 | SERVO_ERROR_OVER_TEMPERATURE = 1 37 | SERVO_ERROR_OVER_VOLTAGE = 2 38 | SERVO_ERROR_LOCKED_ROTOR = 4 39 | 40 | # SERVOS 41 | MOTOR_LEFT_FRONT = 1 42 | MOTOR_LEFT_MIDDLE = 2 43 | MOTOR_LEFT_BACK = 3 44 | MOTOR_RIGHT_FRONT = 4 45 | MOTOR_RIGHT_MIDDLE = 5 46 | MOTOR_RIGHT_BACK = 6 47 | 48 | SERVO_LEFT_FRONT = 7 49 | SERVO_RIGHT_FRONT = 8 50 | SERVO_LEFT_BACK = 9 51 | SERVO_RIGHT_BACK = 10 52 | -------------------------------------------------------------------------------- /rover_motor_controller/rover_motor_controller/lx16a/motor_controller.py: -------------------------------------------------------------------------------- 1 | """ 2 | Lewansoul wrapper. 3 | """ 4 | 5 | from typing import List 6 | 7 | from rover_motor_controller.lx16a.lx16a import LX16A 8 | from rover_motor_controller.lx16a.lx16a_consts import ( 9 | MOTOR_LEFT_FRONT, 10 | MOTOR_LEFT_MIDDLE, 11 | MOTOR_LEFT_BACK, 12 | MOTOR_RIGHT_FRONT, 13 | MOTOR_RIGHT_MIDDLE, 14 | MOTOR_RIGHT_BACK, 15 | SERVO_LEFT_FRONT, 16 | SERVO_RIGHT_FRONT, 17 | SERVO_LEFT_BACK, 18 | SERVO_RIGHT_BACK, 19 | ) 20 | 21 | 22 | class MotorController(object): 23 | """ 24 | MotorController class contains the methods necessary to send commands to 25 | the motor controllers for the corner and drive motors. 26 | """ 27 | 28 | def __init__(self, serial_port: str, baud_rate: int): 29 | 30 | self.lx16a = LX16A(serial_port, baud_rate, timeout=1) 31 | 32 | drive_ticks = [0, 0, 0, 0, 0, 0] 33 | 34 | self.lx16a.set_motor_mode(MOTOR_LEFT_FRONT, drive_ticks[0]) 35 | self.lx16a.set_motor_mode(MOTOR_LEFT_MIDDLE, drive_ticks[1]) 36 | self.lx16a.set_motor_mode(MOTOR_LEFT_BACK, drive_ticks[2]) 37 | self.lx16a.set_motor_mode(MOTOR_RIGHT_FRONT, drive_ticks[3]) 38 | self.lx16a.set_motor_mode(MOTOR_RIGHT_MIDDLE, drive_ticks[4]) 39 | self.lx16a.set_motor_mode(MOTOR_RIGHT_BACK, drive_ticks[5]) 40 | self.lx16a.set_servo_mode(SERVO_LEFT_FRONT) 41 | self.lx16a.set_servo_mode(SERVO_RIGHT_FRONT) 42 | self.lx16a.set_servo_mode(SERVO_LEFT_BACK) 43 | self.lx16a.set_servo_mode(SERVO_RIGHT_BACK) 44 | 45 | def corner_to_position(self, corner_ticks: List[int]) -> None: 46 | """ 47 | Method to send position commands to the corner motors 48 | 49 | :param list corner_ticks: A list of ticks for each of the corner motors to move to 50 | """ 51 | 52 | self.lx16a.move_prepare(SERVO_LEFT_FRONT, corner_ticks[0]) 53 | self.lx16a.move_prepare(SERVO_RIGHT_FRONT, corner_ticks[1]) 54 | self.lx16a.move_prepare(SERVO_LEFT_BACK, corner_ticks[2]) 55 | self.lx16a.move_prepare(SERVO_RIGHT_BACK, corner_ticks[3]) 56 | self.lx16a.move_start() 57 | 58 | def send_motor_duty(self, drive_ticks: List[int]) -> None: 59 | """ 60 | Method to send position commands to the drive motors 61 | 62 | :param list drive_ticks: A list of ticks for each of the drice motors to speed to 63 | """ 64 | 65 | self.lx16a.set_motor_mode(MOTOR_LEFT_FRONT, drive_ticks[0]) 66 | self.lx16a.set_motor_mode(MOTOR_LEFT_MIDDLE, drive_ticks[1]) 67 | self.lx16a.set_motor_mode(MOTOR_LEFT_BACK, drive_ticks[2]) 68 | self.lx16a.set_motor_mode(MOTOR_RIGHT_FRONT, drive_ticks[3]) 69 | self.lx16a.set_motor_mode(MOTOR_RIGHT_MIDDLE, drive_ticks[4]) 70 | self.lx16a.set_motor_mode(MOTOR_RIGHT_BACK, drive_ticks[5]) 71 | 72 | def kill_motors(self) -> None: 73 | """ 74 | Stops drive motors and align corner motors 75 | """ 76 | 77 | # Align corner motors 78 | self.corner_to_position([500, 500, 500, 500]) 79 | # Stop drive motors 80 | self.send_motor_duty([0, 0, 0, 0, 0, 0]) 81 | 82 | def get_corner_position(self, servo_id: int) -> int: 83 | return self.lx16a.get_position(servo_id) 84 | 85 | def get_motor_speed(self, servo_id: int) -> int: 86 | return self.lx16a.get_motor_speed(servo_id) 87 | 88 | def get_mode(self, servo_id: int) -> int: 89 | return self.lx16a.get_mode(servo_id) 90 | 91 | def get_status(self, servo_id: int) -> int: 92 | return self.lx16a.is_motor_on(servo_id) 93 | 94 | def move(self, servo_id: int, position: int, time: int = 1000) -> None: 95 | self.lx16a.move(servo_id, position, time) 96 | 97 | def led_turn_off(self, servo_id: int) -> None: 98 | self.lx16a.led_off(servo_id) 99 | 100 | def led_turn_on(self, servo_id: int) -> None: 101 | self.lx16a.led_on(servo_id) 102 | -------------------------------------------------------------------------------- /rover_motor_controller/rover_motor_controller/motor_controller/__init__.py: -------------------------------------------------------------------------------- 1 | from .controller_node import ControllerNode 2 | from .vel_parser_node import VelParserNode 3 | -------------------------------------------------------------------------------- /rover_motor_controller/rover_motor_controller/motor_controller/controller_node.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | """ 25 | Motor controller node. 26 | """ 27 | 28 | from rclpy.node import Node 29 | 30 | from rover_msgs.msg import MotorsCommand 31 | from rover_motor_controller.lx16a import MotorController 32 | 33 | 34 | class ControllerNode(Node): 35 | 36 | def __init__(self): 37 | super().__init__("controller_node") 38 | 39 | # declaring params 40 | self.declare_parameter("motor_controller_device", "/dev/ttyUSB0") 41 | self.declare_parameter("baud_rate", 115200) 42 | 43 | # getting params 44 | motor_controller_device = ( 45 | self.get_parameter("motor_controller_device") 46 | .get_parameter_value() 47 | .string_value 48 | ) 49 | baud_rate = self.get_parameter("baud_rate").get_parameter_value().integer_value 50 | 51 | self.motor_controller = MotorController(motor_controller_device, baud_rate) 52 | 53 | # sub 54 | self.subscription = self.create_subscription( 55 | MotorsCommand, "motors_command", self.callback, 10 56 | ) 57 | 58 | def callback(self, msg: MotorsCommand) -> None: 59 | """ 60 | Callback function called when a MotorsCommand message is received from /motors_command topic 61 | :param list msg: A list of corner and motor lists values 62 | """ 63 | 64 | # Send angle values to corner motors 65 | self.motor_controller.corner_to_position(msg.corner_motor) 66 | 67 | # Send speed values to drive motors 68 | self.motor_controller.send_motor_duty(msg.drive_motor) 69 | 70 | def shutdown(self) -> None: 71 | """ 72 | Stop motors 73 | """ 74 | 75 | self.get_logger().info("Killing motors") 76 | self.motor_controller.kill_motors() 77 | -------------------------------------------------------------------------------- /rover_motor_controller/rover_motor_controller/vel_parser_node_main.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | import rclpy 25 | from rover_motor_controller.motor_controller import VelParserNode 26 | 27 | 28 | def main(args=None): 29 | rclpy.init(args=args) 30 | 31 | node = VelParserNode() 32 | 33 | rclpy.spin(node) 34 | 35 | rclpy.shutdown() 36 | 37 | 38 | if __name__ == "__main__": 39 | main() 40 | -------------------------------------------------------------------------------- /rover_motor_controller/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script-dir=$base/lib/rover_motor_controller 3 | [install] 4 | install-scripts=$base/lib/rover_motor_controller 5 | -------------------------------------------------------------------------------- /rover_motor_controller/setup.py: -------------------------------------------------------------------------------- 1 | from glob import glob 2 | import os 3 | from setuptools import setup, find_packages 4 | 5 | package_name = "rover_motor_controller" 6 | 7 | setup( 8 | name=package_name, 9 | version="1.1.0", 10 | packages=find_packages(), 11 | data_files=[ 12 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 13 | ("share/" + package_name, ["package.xml"]), 14 | (os.path.join("share/", package_name, "launch/"), glob("launch/*.launch.py")), 15 | ], 16 | install_requires=["setuptools"], 17 | zip_safe=True, 18 | maintainer="Miguel Ángel González Santamarta", 19 | maintainer_email="mgons@unileon.es", 20 | description="Rover motor controller package (Python)", 21 | license="MIT", 22 | tests_require=["pytest"], 23 | entry_points={ 24 | "console_scripts": [ 25 | "controller_node = rover_motor_controller.controller_node_main:main", 26 | "vel_parser_node = rover_motor_controller.vel_parser_node_main:main", 27 | ], 28 | }, 29 | ) 30 | -------------------------------------------------------------------------------- /rover_motor_controller/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=[".", "test"]) 23 | assert rc == 0, "Found errors" 24 | -------------------------------------------------------------------------------- /rover_motor_controller/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join( 24 | errors 25 | ) 26 | -------------------------------------------------------------------------------- /rover_motor_controller/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=[".", "test"]) 23 | assert rc == 0, "Found code style errors / warnings" 24 | -------------------------------------------------------------------------------- /rover_motor_controller_cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(rover_motor_controller_cpp) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(rover_msgs REQUIRED) 22 | find_package(geometry_msgs REQUIRED) 23 | 24 | INCLUDE_DIRECTORIES( 25 | include 26 | ) 27 | 28 | ### CONTROLLER NODE 29 | set(CONTROLLER_NODE_SOURCES 30 | src/controller_node_main.cpp 31 | src/lx16a/serial.cpp 32 | src/lx16a/lx16a.cpp 33 | src/lx16a/motor_controller.cpp 34 | src/motor_controller/controller_node.cpp 35 | ) 36 | add_executable(controller_node ${CONTROLLER_NODE_SOURCES}) 37 | target_link_libraries(controller_node pthread boost_system) 38 | ament_target_dependencies(controller_node rclcpp rover_msgs) 39 | 40 | ### VEL PARSER NODE 41 | set(VEL_PARSER_NODE_SOURCES 42 | src/vel_parser_node_main.cpp 43 | src/motor_controller/vel_parser_node.cpp 44 | ) 45 | add_executable(vel_parser_node ${VEL_PARSER_NODE_SOURCES}) 46 | target_link_libraries(vel_parser_node) 47 | ament_target_dependencies(vel_parser_node rclcpp rover_msgs geometry_msgs) 48 | 49 | # INSTALL 50 | install(TARGETS 51 | controller_node 52 | vel_parser_node 53 | DESTINATION lib/${PROJECT_NAME}) 54 | 55 | # install the launch directory 56 | install(DIRECTORY 57 | launch 58 | DESTINATION share/${PROJECT_NAME}/ 59 | ) 60 | 61 | # TEST 62 | if(BUILD_TESTING) 63 | find_package(ament_cmake_clang_format REQUIRED) 64 | ament_clang_format(CONFIG_FILE .clang-format) 65 | endif() 66 | 67 | ament_package() 68 | -------------------------------------------------------------------------------- /rover_motor_controller_cpp/include/lx16a/lx16a.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef LX16A_HPP 24 | #define LX16A_HPP 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include "lx16a/lx16a_consts.hpp" 33 | #include "lx16a/serial.hpp" 34 | 35 | namespace lx16a { 36 | 37 | class LX16A { 38 | private: 39 | std::unique_ptr serial; 40 | std::unique_ptr mtx; 41 | 42 | void send_command(uint8_t servo_id, uint8_t command, 43 | std::vector params); 44 | std::vector wait_for_response(uint8_t servo_id, uint8_t command); 45 | std::vector query(uint8_t servo_id, uint8_t command); 46 | 47 | public: 48 | LX16A(std::string serial_port, unsigned int baud_rate); 49 | 50 | uint8_t lower_byte(int value); 51 | uint8_t higher_byte(int value); 52 | uint8_t word(int low, int high); 53 | int clamp(int range_min, int range_max, int value); 54 | 55 | uint8_t get_servo_id(uint8_t servo_id); 56 | void set_servo_id(uint8_t servo_id, uint8_t new_servo_id); 57 | 58 | void move(uint8_t servo_id, int position, int time = 0); 59 | void move_prepare(uint8_t servo_id, int position, int time = 0); 60 | void move_start(uint8_t servo_id = SERVO_ID_ALL); 61 | void move_stop(uint8_t servo_id = SERVO_ID_ALL); 62 | 63 | std::vector get_prepared_move(uint8_t servo_id); 64 | int get_position_offset(uint8_t servo_id); 65 | std::vector get_position_limits(int8_t servo_id); 66 | int get_position(uint8_t servo_id); 67 | 68 | void set_position_offset(uint8_t servo_id, int deviation); 69 | void save_position_offset(uint8_t servo_id); 70 | void set_position_limits(uint8_t servo_id, int min_position, 71 | int max_position); 72 | 73 | std::vector get_voltage_limits(uint8_t servo_id); 74 | uint8_t get_voltage(uint8_t servo_id); 75 | void set_voltage_limits(uint8_t servo_id, int min_voltage, int max_voltage); 76 | 77 | uint8_t get_max_temperature_limit(uint8_t servo_id); 78 | uint8_t get_temperature(uint8_t servo_id); 79 | void set_max_temperature_limit(uint8_t servo_id, int max_temperature); 80 | 81 | uint8_t get_mode(uint8_t servo_id); 82 | int get_motor_speed(uint8_t servo_id); 83 | void set_servo_mode(uint8_t servo_id); 84 | void set_motor_mode(uint8_t servo_id, int speed = 0); 85 | bool is_motor_on(uint8_t servo_id); 86 | void motor_on(uint8_t servo_id); 87 | void motor_off(uint8_t servo_id); 88 | 89 | bool is_led_on(uint8_t servo_id); 90 | void led_on(uint8_t servo_id); 91 | void led_off(uint8_t servo_id); 92 | uint8_t get_led_errors(uint8_t servo_id); 93 | void set_led_errors(uint8_t servo_id, uint8_t error); 94 | }; 95 | 96 | } // namespace lx16a 97 | #endif 98 | -------------------------------------------------------------------------------- /rover_motor_controller_cpp/include/lx16a/lx16a_consts.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef LX16A_CONSTS_HPP 24 | #define LX16A_CONSTS_HPP 25 | 26 | #include 27 | 28 | namespace lx16a { 29 | 30 | constexpr uint8_t SERVO_ID_ALL = 0xfe; 31 | constexpr uint8_t SERVO_FRAME_HEADER = 0x55; 32 | 33 | // CMDS 34 | constexpr uint8_t SERVO_MOVE_TIME_WRITE = 1; 35 | constexpr uint8_t SERVO_MOVE_TIME_READ = 2; 36 | constexpr uint8_t SERVO_MOVE_TIME_WAIT_WRITE = 7; 37 | constexpr uint8_t SERVO_MOVE_TIME_WAIT_READ = 8; 38 | constexpr uint8_t SERVO_MOVE_START = 11; 39 | constexpr uint8_t SERVO_MOVE_STOP = 12; 40 | constexpr uint8_t SERVO_ID_WRITE = 13; 41 | constexpr uint8_t SERVO_ID_READ = 14; 42 | constexpr uint8_t SERVO_ANGLE_OFFSET_ADJUST = 17; 43 | constexpr uint8_t SERVO_ANGLE_OFFSET_WRITE = 18; 44 | constexpr uint8_t SERVO_ANGLE_OFFSET_READ = 19; 45 | constexpr uint8_t SERVO_ANGLE_LIMIT_WRITE = 20; 46 | constexpr uint8_t SERVO_ANGLE_LIMIT_READ = 21; 47 | constexpr uint8_t SERVO_VIN_LIMIT_WRITE = 22; 48 | constexpr uint8_t SERVO_VIN_LIMIT_READ = 23; 49 | constexpr uint8_t SERVO_TEMP_MAX_LIMIT_WRITE = 24; 50 | constexpr uint8_t SERVO_TEMP_MAX_LIMIT_READ = 25; 51 | constexpr uint8_t SERVO_TEMP_READ = 26; 52 | constexpr uint8_t SERVO_VIN_READ = 27; 53 | constexpr uint8_t SERVO_POS_READ = 28; 54 | constexpr uint8_t SERVO_OR_MOTOR_MODE_WRITE = 29; 55 | constexpr uint8_t SERVO_OR_MOTOR_MODE_READ = 30; 56 | constexpr uint8_t SERVO_LOAD_OR_UNLOAD_WRITE = 31; 57 | constexpr uint8_t SERVO_LOAD_OR_UNLOAD_READ = 32; 58 | constexpr uint8_t SERVO_LED_CTRL_WRITE = 33; 59 | constexpr uint8_t SERVO_LED_CTRL_READ = 34; 60 | constexpr uint8_t SERVO_LED_ERROR_WRITE = 35; 61 | constexpr uint8_t SERVO_LED_ERROR_READ = 36; 62 | 63 | // ERRORS 64 | constexpr uint8_t SERVO_ERROR_OVER_TEMPERATURE = 1; 65 | constexpr uint8_t SERVO_ERROR_OVER_VOLTAGE = 2; 66 | constexpr uint8_t SERVO_ERROR_LOCKED_ROTOR = 4; 67 | 68 | // SERVOS 69 | constexpr uint8_t MOTOR_LEFT_FRONT = 1; 70 | constexpr uint8_t MOTOR_LEFT_MIDDLE = 2; 71 | constexpr uint8_t MOTOR_LEFT_BACK = 3; 72 | constexpr uint8_t MOTOR_RIGHT_FRONT = 4; 73 | constexpr uint8_t MOTOR_RIGHT_MIDDLE = 5; 74 | constexpr uint8_t MOTOR_RIGHT_BACK = 6; 75 | 76 | constexpr uint8_t SERVO_LEFT_FRONT = 7; 77 | constexpr uint8_t SERVO_RIGHT_FRONT = 8; 78 | constexpr uint8_t SERVO_LEFT_BACK = 9; 79 | constexpr uint8_t SERVO_RIGHT_BACK = 10; 80 | 81 | } // namespace lx16a 82 | #endif 83 | -------------------------------------------------------------------------------- /rover_motor_controller_cpp/include/lx16a/motor_controller.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef MOTOR_CONTROLLER_HPP 24 | #define MOTOR_CONTROLLER_HPP 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "lx16a/lx16a.hpp" 32 | #include "lx16a/lx16a_consts.hpp" 33 | 34 | namespace lx16a { 35 | 36 | class MotorController { 37 | private: 38 | std::unique_ptr lx16a; 39 | 40 | public: 41 | MotorController(std::string serial_port, unsigned int baud_rate); 42 | 43 | void corner_to_position(std::vector corner_ticks); 44 | void send_motor_duty(std::vector drive_ticks); 45 | 46 | void kill_motors(); 47 | 48 | int get_corner_position(uint8_t servo_id); 49 | int get_motor_speed(uint8_t servo_id); 50 | 51 | uint8_t get_mode(uint8_t servo_id); 52 | uint8_t get_status(uint8_t servo_id); 53 | 54 | void move(uint8_t servo_id, int position, int time); 55 | 56 | void led_turn_off(uint8_t servo_id); 57 | void led_turn_on(uint8_t servo_id); 58 | }; 59 | 60 | } // namespace lx16a 61 | #endif 62 | -------------------------------------------------------------------------------- /rover_motor_controller_cpp/include/lx16a/serial.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef SERIAL_HPP 24 | #define SERIAL_HPP 25 | 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | namespace lx16a { 33 | 34 | class Serial { 35 | public: 36 | Serial(std::string device_name, unsigned int baud_rate); 37 | bool connect(); 38 | 39 | bool read(unsigned char &receive_data); 40 | bool read_with_timeout(unsigned char &receive_data, int timeout = 3); 41 | void time_out(const boost::system::error_code &error); 42 | void read_complete(bool &read_error, const boost::system::error_code &error, 43 | size_t bytes_transferred); 44 | 45 | bool write(unsigned char &data); 46 | bool write(const std::vector &data); 47 | 48 | private: 49 | std::string device_name; 50 | unsigned int baud_rate; 51 | std::unique_ptr io_service; 52 | std::unique_ptr serial_port; 53 | std::unique_ptr timer; 54 | }; 55 | 56 | } // namespace lx16a 57 | #endif 58 | -------------------------------------------------------------------------------- /rover_motor_controller_cpp/include/motor_controller/controller_node.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef CONTROLLER_NODE_HPP 24 | #define CONTROLLER_NODE_HPP 25 | 26 | #include 27 | 28 | #include "rclcpp/rclcpp.hpp" 29 | 30 | #include "lx16a/motor_controller.hpp" 31 | #include "rover_msgs/msg/motors_command.hpp" 32 | 33 | namespace motor_controller { 34 | 35 | class ControllerNode : public rclcpp::Node { 36 | public: 37 | ControllerNode(); 38 | void callback(const rover_msgs::msg::MotorsCommand::SharedPtr msg); 39 | void shutdown(); 40 | 41 | private: 42 | std::unique_ptr motor_controller; 43 | rclcpp::Subscription::SharedPtr subscription; 44 | }; 45 | 46 | } // namespace motor_controller 47 | #endif 48 | -------------------------------------------------------------------------------- /rover_motor_controller_cpp/include/motor_controller/vel_parser_node.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #ifndef CONTROLLER_NODE_HPP 24 | #define CONTROLLER_NODE_HPP 25 | 26 | #include "geometry_msgs/msg/twist.hpp" 27 | #include "rclcpp/rclcpp.hpp" 28 | 29 | #include "rover_msgs/msg/motors_command.hpp" 30 | 31 | #define MAX_RADIUS 255 32 | #define MIN_RADIUS 55 33 | 34 | namespace motor_controller { 35 | 36 | class VelParserNode : public rclcpp::Node { 37 | public: 38 | VelParserNode(); 39 | void callback(const geometry_msgs::msg::Twist::SharedPtr msg); 40 | 41 | float normalize(float value, float old_min, float old_max, float new_min, 42 | float new_max); 43 | float deg_to_tick(float deg, float e_min, float e_max); 44 | float radians_to_deg(float radians); 45 | 46 | std::vector calculate_velocity(float velocity, float radius); 47 | std::vector calculate_target_deg(float radius); 48 | std::vector calculate_target_tick(std::vector target_angles); 49 | 50 | private: 51 | rclcpp::Subscription::SharedPtr subscription; 52 | rclcpp::Publisher::SharedPtr publisher; 53 | int speed_factor; 54 | float d1; 55 | float d2; 56 | float d3; 57 | float d4; 58 | int enc_min; 59 | int enc_max; 60 | float enc_mid; 61 | float linear_limit; 62 | float angular_limit; 63 | float angular_factor; 64 | }; 65 | 66 | } // namespace motor_controller 67 | #endif 68 | -------------------------------------------------------------------------------- /rover_motor_controller_cpp/launch/motor_controller.launch.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | from launch import LaunchDescription 25 | from launch_ros.actions import Node 26 | from launch.substitutions import LaunchConfiguration 27 | from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable 28 | 29 | 30 | def generate_launch_description(): 31 | 32 | pkg_name = "rover_motor_controller_cpp" 33 | stdout_linebuf_envvar = SetEnvironmentVariable( 34 | "RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED", "1" 35 | ) 36 | 37 | # 38 | # ARGS 39 | # 40 | hardware_distances = LaunchConfiguration("hardware_distances") 41 | declare_hardware_distances_cmd = DeclareLaunchArgument( 42 | "hardware_distances", 43 | default_value="[23.0, 25.5, 28.5, 26.0]", 44 | description="Rover hardware distances", 45 | ) 46 | 47 | enc_min = LaunchConfiguration("enc_min") 48 | declare_enc_min_cmd = DeclareLaunchArgument( 49 | "enc_min", 50 | default_value="250", 51 | description="enc_min", 52 | ) 53 | 54 | speed_factor = LaunchConfiguration("speed_factor") 55 | declare_speed_factor_cmd = DeclareLaunchArgument( 56 | "speed_factor", 57 | default_value="10", 58 | description="Speed [-100, +100] * 6 = [-1000, +1000]", 59 | ) 60 | 61 | enc_max = LaunchConfiguration("enc_max") 62 | declare_enc_max_cmd = DeclareLaunchArgument( 63 | "enc_max", 64 | default_value="750", 65 | description="enc_max", 66 | ) 67 | 68 | motor_controller_device = LaunchConfiguration("motor_controller_device") 69 | declare_motor_controller_device_cmd = DeclareLaunchArgument( 70 | "motor_controller_device", 71 | default_value="/dev/ttyUSB0", 72 | description="Motor controller device", 73 | ) 74 | 75 | baud_rate = LaunchConfiguration("baud_rate") 76 | declare_baud_rate_cmd = DeclareLaunchArgument( 77 | "baud_rate", 78 | default_value="115200", 79 | description="baud rate", 80 | ) 81 | 82 | # 83 | # NODES 84 | # 85 | 86 | vel_parser_node_cmd = Node( 87 | package=pkg_name, 88 | executable="vel_parser_node", 89 | name="vel_parser_node", 90 | parameters=[ 91 | { 92 | "hardware_distances": hardware_distances, 93 | "enc_min": enc_min, 94 | "enc_max": enc_max, 95 | "speed_factor": speed_factor, 96 | } 97 | ], 98 | ) 99 | 100 | controller_node_cmd = Node( 101 | package=pkg_name, 102 | executable="controller_node", 103 | name="controller_node", 104 | parameters=[ 105 | {"motor_controller_device": motor_controller_device, "baud_rate": baud_rate} 106 | ], 107 | ) 108 | 109 | ld = LaunchDescription() 110 | 111 | ld.add_action(stdout_linebuf_envvar) 112 | 113 | ld.add_action(declare_hardware_distances_cmd) 114 | ld.add_action(declare_enc_min_cmd) 115 | ld.add_action(declare_enc_max_cmd) 116 | ld.add_action(declare_speed_factor_cmd) 117 | ld.add_action(declare_motor_controller_device_cmd) 118 | ld.add_action(declare_baud_rate_cmd) 119 | 120 | ld.add_action(vel_parser_node_cmd) 121 | ld.add_action(controller_node_cmd) 122 | 123 | return ld 124 | -------------------------------------------------------------------------------- /rover_motor_controller_cpp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rover_motor_controller_cpp 5 | 1.1.0 6 | Rover motor controller package (C++) 7 | Miguel Ángel González Santamarta 8 | MIT 9 | ament_cmake 10 | ament_lint_auto 11 | ament_clang_format 12 | ament_cmake_clang_format 13 | rover_msgs 14 | geometry_msgs 15 | 16 | ament_cmake 17 | 18 | -------------------------------------------------------------------------------- /rover_motor_controller_cpp/src/controller_node_main.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | 25 | #include "motor_controller/controller_node.hpp" 26 | 27 | using namespace motor_controller; 28 | 29 | int main(int argc, char *argv[]) { 30 | rclcpp::init(argc, argv); 31 | rclcpp::spin(std::make_shared()); 32 | rclcpp::shutdown(); 33 | return 0; 34 | } 35 | -------------------------------------------------------------------------------- /rover_motor_controller_cpp/src/lx16a/motor_controller.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | #include 25 | 26 | #include "lx16a/lx16a.hpp" 27 | #include "lx16a/lx16a_consts.hpp" 28 | #include "lx16a/motor_controller.hpp" 29 | 30 | using namespace lx16a; 31 | 32 | MotorController::MotorController(std::string serial_port, 33 | unsigned int baud_rate) { 34 | 35 | this->lx16a = std::make_unique(LX16A(serial_port, baud_rate)); 36 | 37 | this->lx16a->set_motor_mode(MOTOR_LEFT_FRONT, 0); 38 | this->lx16a->set_motor_mode(MOTOR_LEFT_MIDDLE, 0); 39 | this->lx16a->set_motor_mode(MOTOR_LEFT_BACK, 0); 40 | this->lx16a->set_motor_mode(MOTOR_RIGHT_FRONT, 0); 41 | this->lx16a->set_motor_mode(MOTOR_RIGHT_MIDDLE, 0); 42 | this->lx16a->set_motor_mode(MOTOR_RIGHT_BACK, 0); 43 | this->lx16a->set_servo_mode(SERVO_LEFT_FRONT); 44 | this->lx16a->set_servo_mode(SERVO_RIGHT_FRONT); 45 | this->lx16a->set_servo_mode(SERVO_LEFT_BACK); 46 | this->lx16a->set_servo_mode(SERVO_RIGHT_BACK); 47 | } 48 | 49 | void MotorController::corner_to_position(std::vector corner_ticks) { 50 | this->lx16a->move_prepare(SERVO_LEFT_FRONT, corner_ticks[0], 0); 51 | this->lx16a->move_prepare(SERVO_RIGHT_FRONT, corner_ticks[1], 0); 52 | this->lx16a->move_prepare(SERVO_LEFT_BACK, corner_ticks[2], 0); 53 | this->lx16a->move_prepare(SERVO_RIGHT_BACK, corner_ticks[3], 0); 54 | this->lx16a->move_start(SERVO_ID_ALL); 55 | } 56 | 57 | void MotorController::send_motor_duty(std::vector drive_ticks) { 58 | this->lx16a->set_motor_mode(MOTOR_LEFT_FRONT, drive_ticks[0]); 59 | this->lx16a->set_motor_mode(MOTOR_LEFT_MIDDLE, drive_ticks[1]); 60 | this->lx16a->set_motor_mode(MOTOR_LEFT_BACK, drive_ticks[2]); 61 | this->lx16a->set_motor_mode(MOTOR_RIGHT_FRONT, drive_ticks[3]); 62 | this->lx16a->set_motor_mode(MOTOR_RIGHT_MIDDLE, drive_ticks[4]); 63 | this->lx16a->set_motor_mode(MOTOR_RIGHT_BACK, drive_ticks[5]); 64 | } 65 | 66 | void MotorController::kill_motors() { 67 | this->corner_to_position({500, 500, 500, 500}); 68 | this->send_motor_duty({0, 0, 0, 0, 0, 0}); 69 | } 70 | 71 | int MotorController::get_corner_position(uint8_t servo_id) { 72 | return this->lx16a->get_position(servo_id); 73 | } 74 | 75 | int MotorController::get_motor_speed(uint8_t servo_id) { 76 | return this->lx16a->get_motor_speed(servo_id); 77 | } 78 | 79 | uint8_t MotorController::get_mode(uint8_t servo_id) { 80 | return this->lx16a->get_mode(servo_id); 81 | } 82 | 83 | uint8_t MotorController::get_status(uint8_t servo_id) { 84 | return this->lx16a->is_motor_on(servo_id); 85 | } 86 | 87 | void MotorController::move(uint8_t servo_id, int position, int time) { 88 | this->lx16a->move(servo_id, position, time); 89 | } 90 | 91 | void MotorController::led_turn_off(uint8_t servo_id) { 92 | this->lx16a->led_off(servo_id); 93 | } 94 | 95 | void MotorController::led_turn_on(uint8_t servo_id) { 96 | this->lx16a->led_on(servo_id); 97 | } 98 | -------------------------------------------------------------------------------- /rover_motor_controller_cpp/src/lx16a/serial.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | #include 25 | 26 | #include "lx16a/serial.hpp" 27 | 28 | using namespace lx16a; 29 | 30 | Serial::Serial(std::string device_name, unsigned int baud_rate) { 31 | 32 | this->device_name = device_name; 33 | this->baud_rate = baud_rate; 34 | 35 | this->io_service = std::make_unique(); 36 | this->serial_port = std::make_unique( 37 | boost::asio::serial_port(*this->io_service)); 38 | this->timer = 39 | std::make_unique(*this->io_service); 40 | } 41 | 42 | bool Serial::connect() { 43 | 44 | try { 45 | this->serial_port->open(this->device_name); 46 | 47 | // flow control 48 | boost::asio::serial_port_base::flow_control flowCrtl( 49 | boost::asio::serial_port_base::flow_control::none); 50 | this->serial_port->set_option(flowCrtl); 51 | 52 | // setting baud_rate 53 | this->serial_port->set_option( 54 | boost::asio::serial_port_base::baud_rate(this->baud_rate)); 55 | 56 | // charsize 57 | boost::asio::serial_port_base::stop_bits stopBits_1( 58 | boost::asio::serial_port_base::stop_bits::one); 59 | this->serial_port->set_option(stopBits_1); 60 | 61 | boost::asio::serial_port_base::parity parityBits_0( 62 | boost::asio::serial_port_base::parity::none); 63 | this->serial_port->set_option(parityBits_0); 64 | 65 | unsigned int charsize = 8; 66 | this->serial_port->set_option( 67 | boost::asio::serial_port_base::character_size(charsize)); 68 | 69 | } catch (boost::system::system_error &error) { 70 | return false; 71 | } 72 | 73 | return true; 74 | } 75 | 76 | bool Serial::read(unsigned char &receive_data) { 77 | this->serial_port->read_some(boost::asio::buffer(&receive_data, 1)); 78 | return true; 79 | } 80 | 81 | void Serial::read_complete(bool &read_error, 82 | const boost::system::error_code &error, 83 | size_t bytes_transferred) { 84 | 85 | read_error = (error || bytes_transferred == 0); 86 | this->timer->cancel(); 87 | } 88 | 89 | void Serial::time_out(const boost::system::error_code &error) { 90 | 91 | if (error) { 92 | return; 93 | } 94 | 95 | this->serial_port->cancel(); 96 | } 97 | 98 | bool Serial::read_with_timeout(unsigned char &receive_data, int timeout) { 99 | 100 | if (!timeout) { 101 | return this->read(receive_data); 102 | } 103 | 104 | unsigned char buffer[1]; 105 | bool read_error = true; 106 | 107 | // asynchronously read 1 char 108 | this->serial_port->async_read_some( 109 | boost::asio::buffer(buffer, 1), 110 | boost::bind(&Serial::read_complete, this, boost::ref(read_error), 111 | boost::asio::placeholders::error, 112 | boost::asio::placeholders::bytes_transferred)); 113 | 114 | // setup a deadline time to implement our timeout. 115 | this->timer->expires_from_now(boost::posix_time::seconds(timeout)); 116 | this->timer->async_wait( 117 | boost::bind(&Serial::time_out, this, boost::asio::placeholders::error)); 118 | 119 | // block until a character is read or until it is cancelled. 120 | this->io_service->run(); 121 | 122 | // reset after a timeout and cancel 123 | this->io_service->reset(); 124 | 125 | if (!read_error) 126 | receive_data = buffer[0]; 127 | 128 | return !read_error; 129 | } 130 | 131 | bool Serial::write(unsigned char &data) { 132 | this->serial_port->write_some(boost::asio::buffer(&data, 1)); 133 | return true; 134 | } 135 | 136 | bool Serial::write(const std::vector &data) { 137 | this->serial_port->write_some( 138 | boost::asio::buffer(&data.front(), data.size())); 139 | return true; 140 | } -------------------------------------------------------------------------------- /rover_motor_controller_cpp/src/motor_controller/controller_node.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #define BOOST_BIND_NO_PLACEHOLDERS 24 | 25 | #include 26 | #include 27 | 28 | #include "rclcpp/rclcpp.hpp" 29 | 30 | #include "lx16a/motor_controller.hpp" 31 | #include "motor_controller/controller_node.hpp" 32 | #include "rover_msgs/msg/motors_command.hpp" 33 | 34 | using std::placeholders::_1; 35 | using namespace motor_controller; 36 | 37 | ControllerNode::ControllerNode() : rclcpp::Node("controller_node") { 38 | 39 | // declaring params 40 | this->declare_parameter("motor_controller_device", 41 | "/dev/ttyUSB0"); 42 | this->declare_parameter("baud_rate", 115200); 43 | 44 | // getting params 45 | std::string motor_controller_device; 46 | this->get_parameter("motor_controller_device", motor_controller_device); 47 | 48 | int baud_rate; 49 | this->get_parameter("baud_rate", baud_rate); 50 | 51 | this->motor_controller = std::make_unique( 52 | lx16a::MotorController(motor_controller_device, baud_rate)); 53 | 54 | // sub 55 | this->subscription = 56 | this->create_subscription( 57 | "motors_command", 10, std::bind(&ControllerNode::callback, this, _1)); 58 | } 59 | 60 | void ControllerNode::callback( 61 | const rover_msgs::msg::MotorsCommand::SharedPtr msg) { 62 | this->motor_controller->corner_to_position(msg->corner_motor); 63 | this->motor_controller->send_motor_duty(msg->drive_motor); 64 | } 65 | 66 | void ControllerNode::shutdown() { this->motor_controller->kill_motors(); } -------------------------------------------------------------------------------- /rover_motor_controller_cpp/src/vel_parser_node_main.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | 23 | #include 24 | 25 | #include "motor_controller/vel_parser_node.hpp" 26 | 27 | using namespace motor_controller; 28 | 29 | int main(int argc, char *argv[]) { 30 | rclcpp::init(argc, argv); 31 | rclcpp::spin(std::make_shared()); 32 | rclcpp::shutdown(); 33 | return 0; 34 | } 35 | -------------------------------------------------------------------------------- /rover_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(rover_msgs) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | find_package(rosidl_default_generators REQUIRED) 7 | 8 | rosidl_generate_interfaces(${PROJECT_NAME} 9 | "msg/MotorsCommand.msg" 10 | ) 11 | 12 | ament_package() 13 | -------------------------------------------------------------------------------- /rover_msgs/msg/MotorsCommand.msg: -------------------------------------------------------------------------------- 1 | int32[] drive_motor 2 | int32[] corner_motor -------------------------------------------------------------------------------- /rover_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rover_msgs 5 | 1.1.0 6 | Msgs package for rover 7 | Miguel Ángel González Santamarta 8 | MIT 9 | ament_cmake 10 | rosidl_interface_packages 11 | 12 | ament_cmake 13 | rosidl_default_generators 14 | rosidl_default_runtime 15 | 16 | -------------------------------------------------------------------------------- /rover_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(rover_navigation) 3 | 4 | # Find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | 7 | install( 8 | DIRECTORY launch params behavior_trees 9 | DESTINATION share/${PROJECT_NAME} 10 | ) 11 | 12 | ament_package() -------------------------------------------------------------------------------- /rover_navigation/behavior_trees/rover_bt.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /rover_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rover_navigation 5 | 1.1.0 6 | Rover Navigation2 package 7 | Miguel Ángel González Santamarta 8 | MIT 9 | ament_cmake 10 | rclcpp 11 | rclpy 12 | tf2_ros 13 | tf_transformations 14 | geometry_msgs 15 | nav2_msgs 16 | nav2_bringup 17 | navigation2 18 | nav2_smac_planner 19 | nav2_regulated_pure_pursuit_controller 20 | 21 | ament_cmake 22 | 23 | -------------------------------------------------------------------------------- /rover_service/install.sh: -------------------------------------------------------------------------------- 1 | # dependencies 2 | echo "--------INSTALLING DEPENDENCIES" 3 | apt install ros-humble-joy-linux ros-humble-teleop-twist-joy ros-humble-urg-node -y >>/dev/null 4 | 5 | # copy rover project 6 | echo "--------COPYING SH FILE" 7 | cp rover.sh /usr/local/bin/rover.sh >>/dev/null 8 | 9 | # copy rover service 10 | echo "--------COPYING SERVICE FILE" 11 | cp rover.service /etc/systemd/system/rover.service 12 | 13 | # add permissions 14 | echo "--------ADDING PERMISSIONS" 15 | chmod 744 /usr/local/bin/rover.sh 16 | chmod 664 /etc/systemd/system/rover.service 17 | 18 | # enable service 19 | echo "--------ENABLING SERVICE" 20 | systemctl daemon-reload 21 | systemctl enable rover.service 22 | 23 | echo "--------STARTING SERVICE" 24 | # start service 25 | systemctl start rover 26 | -------------------------------------------------------------------------------- /rover_service/rover.service: -------------------------------------------------------------------------------- 1 | [Unit] 2 | After=network.service 3 | 4 | [Service] 5 | User=ubuntu 6 | Group=ubuntu 7 | ExecStart=/bin/bash /usr/local/bin/rover.sh 8 | 9 | [Install] 10 | WantedBy=default.target 11 | -------------------------------------------------------------------------------- /rover_service/rover.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | source /opt/ros/humble/setup.bash 3 | source ~/ros2_ws/install/setup.bash 4 | ros2 launch rover_bringup rover.launch.py 5 | -------------------------------------------------------------------------------- /rover_teleop/config/ps3.yaml: -------------------------------------------------------------------------------- 1 | teleop_twist_joy_node: 2 | ros__parameters: 3 | axis_linear: 4 | x: 1 5 | scale_linear: 6 | x: 0.5 7 | scale_linear_turbo: 8 | x: 1.0 9 | 10 | axis_angular: 11 | yaw: 0 12 | scale_angular: 13 | yaw: 0.5 14 | scale_angular_turbo: 15 | yaw: 1.0 16 | 17 | enable_button: 8 # L2 shoulder button 18 | enable_turbo_button: 10 # L1 shoulder button 19 | -------------------------------------------------------------------------------- /rover_teleop/launch/joy_teleop.launch.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | import os 25 | from ament_index_python.packages import get_package_share_directory 26 | import launch 27 | import launch_ros.actions 28 | 29 | 30 | def generate_launch_description(): 31 | joy_config = launch.substitutions.LaunchConfiguration("joy_config") 32 | joy_dev = launch.substitutions.LaunchConfiguration("joy_dev") 33 | config_filepath = launch.substitutions.LaunchConfiguration("config_filepath") 34 | 35 | return launch.LaunchDescription( 36 | [ 37 | launch.actions.DeclareLaunchArgument("joy_config", default_value="ps3"), 38 | launch.actions.DeclareLaunchArgument( 39 | "joy_dev", default_value="/dev/input/js0" 40 | ), 41 | launch.actions.DeclareLaunchArgument( 42 | "config_filepath", 43 | default_value=[ 44 | launch.substitutions.TextSubstitution( 45 | text=os.path.join( 46 | get_package_share_directory("rover_teleop"), "config", "" 47 | ) 48 | ), 49 | joy_config, 50 | launch.substitutions.TextSubstitution(text=".yaml"), 51 | ], 52 | ), 53 | launch_ros.actions.Node( 54 | package="joy_linux", 55 | executable="joy_linux_node", 56 | name="joy_linux_node", 57 | parameters=[ 58 | { 59 | "dev": joy_dev, 60 | "deadzone": 0.3, 61 | "autorepeat_rate": 0.0, 62 | } 63 | ], 64 | ), 65 | launch_ros.actions.Node( 66 | package="teleop_twist_joy", 67 | executable="teleop_node", 68 | name="teleop_twist_joy_node", 69 | parameters=[config_filepath], 70 | ), 71 | ] 72 | ) 73 | -------------------------------------------------------------------------------- /rover_teleop/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rover_teleop 5 | 1.1.0 6 | Rover teleop package 7 | Miguel Ángel González Santamarta 8 | MIT 9 | ament_copyright 10 | ament_flake8 11 | ament_pep257 12 | python3-pytest 13 | 14 | ament_python 15 | 16 | -------------------------------------------------------------------------------- /rover_teleop/resource/rover_teleop: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_teleop/resource/rover_teleop -------------------------------------------------------------------------------- /rover_teleop/rover_teleop/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mgonzs13/ros2_rover/5ca8d6e9b8d10064eef1de4e864c76f0cdf6cd89/rover_teleop/rover_teleop/__init__.py -------------------------------------------------------------------------------- /rover_teleop/rover_teleop/teleop_keyboard_node.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Miguel Ángel González Santamarta 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | import sys 25 | import geometry_msgs.msg 26 | import rclpy 27 | import termios 28 | import tty 29 | 30 | 31 | msg = """ 32 | This node takes keypresses from the keyboard and publishes them 33 | as Twist messages. 34 | --------------------------- 35 | Moving around: 36 | w 37 | a s d 38 | x 39 | --------------------------- 40 | """ 41 | 42 | 43 | move_bindings = { 44 | "a": (0, 0, 0, 1), 45 | "w": (1, 0, 0, 0), 46 | "s": (0, 0, 0, 0), 47 | "d": (0, 0, 0, -1), 48 | "x": (-1, 0, 0, 0), 49 | } 50 | 51 | 52 | def getKey(settings): 53 | tty.setraw(sys.stdin.fileno()) 54 | key = sys.stdin.read(1) 55 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 56 | return key 57 | 58 | 59 | def saveTerminalSettings(): 60 | return termios.tcgetattr(sys.stdin) 61 | 62 | 63 | def restoreTerminalSettings(old_settings): 64 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings) 65 | 66 | 67 | def vels(x, z): 68 | return "currently:\tspeed %s\tturn %s " % (round(x, 2), round(z, 2)) 69 | 70 | 71 | def limit(value, l): 72 | if value > l: 73 | return l 74 | 75 | elif value < -l: 76 | return -l 77 | 78 | else: 79 | return value 80 | 81 | 82 | def main(): 83 | settings = saveTerminalSettings() 84 | 85 | rclpy.init() 86 | 87 | node = rclpy.create_node("teleop_keyboard_node") 88 | pub = node.create_publisher(geometry_msgs.msg.Twist, "cmd_vel", 10) 89 | 90 | x = 0.0 91 | th = 0.0 92 | x_factor = 0.1 93 | th_factor = 0.1 94 | 95 | status = 0 96 | 97 | try: 98 | print(msg) 99 | print(vels(x, th)) 100 | 101 | while True: 102 | key = getKey(settings) 103 | if key in move_bindings.keys(): 104 | 105 | x = limit(x + move_bindings[key][0] * x_factor, 1.0) 106 | th = limit(th + move_bindings[key][3] * th_factor, 1.0) 107 | 108 | if 1 not in move_bindings[key] and -1 not in move_bindings[key]: 109 | x = 0.0 110 | th = 0.0 111 | 112 | if status == 14: 113 | print(msg) 114 | status = (status + 1) % 15 115 | 116 | print(vels(x, th)) 117 | 118 | elif key == "\x03": 119 | break 120 | 121 | twist = geometry_msgs.msg.Twist() 122 | twist.linear.x = x 123 | twist.linear.y = 0.0 124 | twist.linear.z = 0.0 125 | twist.angular.x = 0.0 126 | twist.angular.y = 0.0 127 | twist.angular.z = th 128 | pub.publish(twist) 129 | 130 | except Exception as e: 131 | print(e) 132 | 133 | finally: 134 | twist = geometry_msgs.msg.Twist() 135 | twist.linear.x = 0.0 136 | twist.linear.y = 0.0 137 | twist.linear.z = 0.0 138 | twist.angular.x = 0.0 139 | twist.angular.y = 0.0 140 | twist.angular.z = 0.0 141 | pub.publish(twist) 142 | 143 | restoreTerminalSettings(settings) 144 | 145 | 146 | if __name__ == "__main__": 147 | main() 148 | -------------------------------------------------------------------------------- /rover_teleop/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script-dir=$base/lib/rover_teleop 3 | [install] 4 | install-scripts=$base/lib/rover_teleop 5 | -------------------------------------------------------------------------------- /rover_teleop/setup.py: -------------------------------------------------------------------------------- 1 | from glob import glob 2 | import os 3 | from setuptools import setup, find_packages 4 | 5 | package_name = "rover_teleop" 6 | 7 | setup( 8 | name=package_name, 9 | version="1.1.0", 10 | packages=find_packages(), 11 | data_files=[ 12 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 13 | ("share/" + package_name, ["package.xml"]), 14 | (os.path.join("share/", package_name, "launch/"), glob("launch/*.launch.py")), 15 | (os.path.join("share/", package_name, "config/"), glob("config/*.yaml")), 16 | ], 17 | install_requires=["setuptools"], 18 | zip_safe=True, 19 | maintainer="Miguel Ángel González Santamarta", 20 | maintainer_email="mgons@unileon.es", 21 | description="Rover teleop package", 22 | license="MIT", 23 | tests_require=["pytest"], 24 | entry_points={ 25 | "console_scripts": [ 26 | "teleop_keyboard_node = rover_teleop.teleop_keyboard_node:main", 27 | ], 28 | }, 29 | ) 30 | -------------------------------------------------------------------------------- /rover_teleop/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=[".", "test"]) 23 | assert rc == 0, "Found errors" 24 | -------------------------------------------------------------------------------- /rover_teleop/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join( 24 | errors 25 | ) 26 | -------------------------------------------------------------------------------- /rover_teleop/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=[".", "test"]) 23 | assert rc == 0, "Found code style errors / warnings" 24 | --------------------------------------------------------------------------------