├── .gitignore ├── README.md ├── img ├── arch.png ├── arquitectura.png ├── bot_pico.png ├── control.png ├── down.jpg ├── front.jpg ├── left.jpg ├── robot.png ├── top_pico.png └── up.jpg ├── rpi_robot_action_interfaces ├── CMakeLists.txt ├── action │ └── GoToGoal.action └── package.xml ├── rpi_robot_bringup ├── CMakeLists.txt ├── config │ └── ps4.config.yaml ├── include │ └── rpi_robot_bringup │ │ └── basic_data.hpp ├── launch │ ├── lidar.launch.py │ ├── rpi_robot.launch.py │ ├── rpi_robot_joy_teleop.launch.py │ └── rpi_robot_nostate.launch.py ├── package.xml ├── rpi_robot_bringup │ └── __init__.py ├── scripts │ ├── rpi_robot_control.py │ ├── rpi_robot_control_nostate.py │ └── rpi_robot_echo.py └── src │ └── basic_node.cpp ├── rpi_robot_control ├── package.xml ├── resource │ └── rpi_robot_control ├── rpi_robot_control │ ├── __init__.py │ ├── go_to_goal.py │ ├── lidar.py │ └── pid.py ├── setup.cfg ├── setup.py └── test │ ├── test_copyright.py │ ├── test_flake8.py │ └── test_pep257.py └── rpi_robot_slam ├── CMakeLists.txt ├── config └── rpi_robot_lds_2d.lua ├── launch ├── cartographer.launch.py ├── occupancy_grid.launch.py └── rpi_robot_slam.launch.py └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 98 | __pypackages__/ 99 | 100 | # Celery stuff 101 | celerybeat-schedule 102 | celerybeat.pid 103 | 104 | # SageMath parsed files 105 | *.sage.py 106 | 107 | # Environments 108 | .env 109 | .venv 110 | env/ 111 | venv/ 112 | ENV/ 113 | env.bak/ 114 | venv.bak/ 115 | 116 | # Spyder project settings 117 | .spyderproject 118 | .spyproject 119 | 120 | # Rope project settings 121 | .ropeproject 122 | 123 | # mkdocs documentation 124 | /site 125 | 126 | # mypy 127 | .mypy_cache/ 128 | .dmypy.json 129 | dmypy.json 130 | 131 | # Pyre type checker 132 | .pyre/ 133 | 134 | # pytype static type analyzer 135 | .pytype/ 136 | 137 | # Cython debug symbols 138 | cython_debug/ 139 | 140 | # Prerequisites 141 | *.d 142 | 143 | # Compiled Object files 144 | *.slo 145 | *.lo 146 | *.o 147 | *.obj 148 | 149 | # Precompiled Headers 150 | *.gch 151 | *.pch 152 | 153 | # Compiled Dynamic libraries 154 | *.so 155 | *.dylib 156 | *.dll 157 | 158 | # Fortran module files 159 | *.mod 160 | *.smod 161 | 162 | # Compiled Static libraries 163 | *.lai 164 | *.la 165 | *.a 166 | *.lib 167 | 168 | # Executables 169 | *.exe 170 | *.out 171 | *.app -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rpi robot ros2 packages 2 | 3 | > This project is part of the subject "Autonomous Mobile Robots - SSC5888", Sao Paulo University - PPG CCMC. 4 | 5 | > Student: Jose Eduardo Laruta Espejo 6 | 7 | > Professor: Fernando Santos Osorio PhD. 8 | 9 | This repo has all the ros2 packages needed for running a reactive navigation system based 10 | on feedback control. 11 | 12 | ## Robot architecture 13 | ![architecture](img/arquitectura.png) 14 | The robot is organized in a layered architecture in which the low level and hardware interface is implemented 15 | in an embedded system which has the responsibility of real-time control, this low level interface is controlled by a 16 | microcontroller (Raspberry pi Pico). Then, the high level control is implemented using ROS2 Foxy through different packages, nodes 17 | and launchfiles. This high level control is running on a Raspberry Pi 4 single board computer. 18 | 19 | ### Robot hardware 20 | The robot was built using two main computing plaforms for different cases: 21 | 22 | 1. **Real-Time control**: This part was implemented in a Raspberry pi Pico Microcontroller. The microcontroller is in charge of interfacing with the DC motors for traction through a motor driver, it uses PWM signals and digital I/O for this. Also, the microcontroller reads the pulses from the two magnetic incremental encoders. With this pulses and a kinematic model, the odometry of the robot is computed. Then, the microcontroller also implements a simple serial interface for receiving velocity commands and sending the odometry information back. 23 | ![top_pico](img/top_pico.png) 24 | ![bot_pico](img/bot_pico.png) 25 | All the firmware and low level code was made using C++ with the official Raspberry pi Pico SDK. The code for this module can be found in this [repo](https://github.com/tabris2015/differential-drive-rpi-pico) 26 | 27 | 2. **High Level control**: The high level control was implemented in a Raspberry pi Single board computer with the needed compute capabilities for running a full fledged linux distribution (Ubuntu 20.04) and the base ROS2 stack (Foxy). The computer is connected to a RPlidar sensor through USB and to the microcontroller to USB with a virtual serial port. 28 | ![robot](img/robot.png) 29 | 30 | Relative to other hardware aspects, a single 11.1V lipo batter was used to power the entire system: motors, microcontroller, single board computer, with the help of a buck converter that transforms the incoming voltage from the battery to steady 5v. Furthermore, the robot chassis consist of two 3d printed parts printed in PLA, apart from that some nuts an bolts were used. 31 | 32 | ![arch](img/arch.png) 33 | 34 | ## Control architecture 35 | ![control](img/control.png) 36 | The control architecture for the robot is based on purely reactive behaviors implemented in a series of feedback loops. 37 | In the figure, we can observe how the references are computed in a hierarchical way. There are two main control loops implemented: 38 | 39 | - **Wheel velocities control**: Each motor is controlled by a PID controller which receives a reference angular velocity in rad/s and that has a sample time of 20ms. These controllers are running inside the Raspberry pi Pico microcontroller that controls the motor driver directly and receives the pulses from the corresponding magnetic encoders present in each motor shaft. 40 | - **Differential drive kinematics**: The microcontroller only receives linear (m/s) and angular(rad/s) velocities and there is an intermediate step that transforms these commands to the corresponding left and right desired angular velocity. A simplified differential drive model is used for this purpose. 41 | - **Orientation PID**: Since the most critical parameter of motion of the robot is the orientation or heading, another PID controller is in charge of moving the robot to the desired orientation. This controller takes a desired heading angle and compares it to the current heading of the robot and sends a desired angular velocity to the lower layer. 42 | - **Linear Velocity Control Law**: For the linear velocity reference, a exponential function is used. This function returns a velocity that decreases proportional to the distance to the current goal. The control law is given by the following formula: $ K = \frac{v_0(1 - e^{-\alpha dist^2})}{dist}$ 43 | 44 | ### Robot ROS2 packages 45 | The high level control consists of several ROS2 packages: 46 | 47 | #### [rpi_robot_bringup](rpi_robot_bringup) 48 | This package contains the most basic low level interface with the microcontroller. This package has the following 49 | major components: 50 | 51 | - **rpi_robot_control:** Node that establishes a serial connection with the microcontroller in order to send velocity commands 52 | (in form of linear and angular velocity), and to retrieve the state of the robot. The state includes position and orientation from odometry calculation and current linear and angular velocities. A simplified differential drive to unicycle model was used for these calculations. 53 | - **rpi_robot.launch.py:** Launchfile that runs both the control node and the lidar node. The lidar node connects with the 2D lidar sensor and publishes a laserscan message to the /scan topic. The launchfile also inits a static transform publisher for publishing the static transform between the base link of the robot and the laser scanner. 54 | - **rpi_robot_joy_teleop.launch.py:** This launchfile runs the joy node for connecting with a joystick and then the teleop_joy node with a custom config file mapped for using a DualShock4 controller. This launchfile gives teleoperation capabilities to the robot. 55 | 56 | The odometry and teleop functionality can be observed in the following video: 57 | 58 | -- 59 | IMAGE ALT TEXT HERE 62 | -- 63 | 64 | #### [rpi_robot_action_interfaces](rpi_robot_action_interfaces) 65 | This robot uses ROS actions in order to get the desired position of the robot for navigation. This package defines a custom action interface for controlling the position of the robot, this action interface has the following structure: 66 | 67 | - **Goal**: it has two float numbers representing position x, y (in meters) for the desired robot position. 68 | - **Result**: it only contains a boolean value that becomes true if the robot has reached the goal as intended. 69 | - **Feedback**: the feedback message contains the current position, x, y and the distance remaining to the goal in meters for monitoring purposes. With this continous feedback we can plot the trajectories or analyze the behavior of the robot. 70 | 71 | Once this package is built using colcon tool, the action interface is available in the current workspace. 72 | 73 | 74 | 75 | #### [rpi_robot_control](rpi_robot_control) 76 | This package implements a high level reactive-based control for navigating the robot. It has two defined behaviors: 77 | 78 | - **Go-to-goal**: this behavior generates a vector for the robot to pursue that indicates the direction in which the goal is located. The resulting vector is used to compute the reference to the heading and the linear velocity for the robot. 79 | - **Avoid obstacle**: this behavior generates a vector that directs the robot away from an obstacle if the obstacle is too close and to follow the wall if the robot is at a reasonable distance. 80 | 81 | The linear and angular velocity references are sent to the robot publishing in the /cmd_vel topic and the low level control node send these commands through a serial port to the microcontroller. 82 | 83 | All these behavior is executed inside a ROS node that also creates an action server that receives goal commands described previously and also provides feedback. 84 | 85 | This component are implemented in a single node called go_to_goal.py. There are also two python modules with a PID controller implementation and also with a lidar abstraction layer for the avoid obstacle vector calculation. 86 | 87 | A demo of the navigation can be observed in the following video: 88 | 89 | -- 90 | IMAGE ALT TEXT HERE 93 | -- 94 | 95 | 96 | #### [rpi_robot_slam](rpi_robot_slam) 97 | This package contains a set of launchfiles for running a slam task using google cartographer package together with the laser scan readings from the robot and the joystick teleoperation packages. The original goal was to use the map generated with this package to localize and navigate the robot but this was not accomplished due to time limitations. 98 | 99 | This functionality is shown in the folloWing video: 100 | 101 | -- 102 | IMAGE ALT TEXT HERE 105 | -------------------------------------------------------------------------------- /img/arch.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tabris2015/rpi-robot-ros-foxy/a9e297856d414b0550d402342a7fd51b18df320d/img/arch.png -------------------------------------------------------------------------------- /img/arquitectura.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tabris2015/rpi-robot-ros-foxy/a9e297856d414b0550d402342a7fd51b18df320d/img/arquitectura.png -------------------------------------------------------------------------------- /img/bot_pico.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tabris2015/rpi-robot-ros-foxy/a9e297856d414b0550d402342a7fd51b18df320d/img/bot_pico.png -------------------------------------------------------------------------------- /img/control.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tabris2015/rpi-robot-ros-foxy/a9e297856d414b0550d402342a7fd51b18df320d/img/control.png -------------------------------------------------------------------------------- /img/down.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tabris2015/rpi-robot-ros-foxy/a9e297856d414b0550d402342a7fd51b18df320d/img/down.jpg -------------------------------------------------------------------------------- /img/front.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tabris2015/rpi-robot-ros-foxy/a9e297856d414b0550d402342a7fd51b18df320d/img/front.jpg -------------------------------------------------------------------------------- /img/left.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tabris2015/rpi-robot-ros-foxy/a9e297856d414b0550d402342a7fd51b18df320d/img/left.jpg -------------------------------------------------------------------------------- /img/robot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tabris2015/rpi-robot-ros-foxy/a9e297856d414b0550d402342a7fd51b18df320d/img/robot.png -------------------------------------------------------------------------------- /img/top_pico.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tabris2015/rpi-robot-ros-foxy/a9e297856d414b0550d402342a7fd51b18df320d/img/top_pico.png -------------------------------------------------------------------------------- /img/up.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tabris2015/rpi-robot-ros-foxy/a9e297856d414b0550d402342a7fd51b18df320d/img/up.jpg -------------------------------------------------------------------------------- /rpi_robot_action_interfaces/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(rpi_robot_action_interfaces) 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 | # uncomment the following section in order to fill in 21 | # further dependencies manually. 22 | # find_package( REQUIRED) 23 | 24 | if(BUILD_TESTING) 25 | find_package(ament_lint_auto REQUIRED) 26 | # the following line skips the linter which checks for copyrights 27 | # uncomment the line when a copyright and license is not present in all source files 28 | #set(ament_cmake_copyright_FOUND TRUE) 29 | # the following line skips cpplint (only works in a git repo) 30 | # uncomment the line when this package is not in a git repo 31 | #set(ament_cmake_cpplint_FOUND TRUE) 32 | ament_lint_auto_find_test_dependencies() 33 | endif() 34 | 35 | # actions 36 | find_package(rosidl_default_generators REQUIRED) 37 | 38 | rosidl_generate_interfaces(${PROJECT_NAME} 39 | "action/GoToGoal.action" 40 | ) 41 | 42 | ament_package() 43 | -------------------------------------------------------------------------------- /rpi_robot_action_interfaces/action/GoToGoal.action: -------------------------------------------------------------------------------- 1 | # goal 2 | float32 x 3 | float32 y 4 | --- 5 | # result 6 | bool goal_reached 7 | --- 8 | # feedback 9 | float32 current_x 10 | float32 current_y 11 | float32 distance 12 | -------------------------------------------------------------------------------- /rpi_robot_action_interfaces/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rpi_robot_action_interfaces 5 | 0.0.0 6 | TODO: Package description 7 | pepe 8 | TODO: License declaration 9 | 10 | rosidl_default_generators 11 | 12 | action_msgs 13 | 14 | rosidl_interface_packages 15 | 16 | ament_cmake 17 | 18 | ament_lint_auto 19 | ament_lint_common 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /rpi_robot_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(rpi_robot_bringup) 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(ament_cmake_python REQUIRED) 21 | find_package(rclcpp REQUIRED) 22 | find_package(rclpy REQUIRED) 23 | 24 | include_directories(include) 25 | 26 | add_executable(basic_node src/basic_node.cpp) 27 | ament_target_dependencies(basic_node rclcpp) 28 | 29 | install(TARGETS 30 | basic_node 31 | DESTINATION lib/${PROJECT_NAME} 32 | ) 33 | 34 | install( 35 | DIRECTORY launch config 36 | DESTINATION share/${PROJECT_NAME} 37 | ) 38 | 39 | ## python nodes 40 | # ----------------------------------------------- 41 | ament_python_install_package(${PROJECT_NAME}) 42 | 43 | install(PROGRAMS 44 | scripts/rpi_robot_echo.py 45 | scripts/rpi_robot_control.py 46 | scripts/rpi_robot_control_nostate.py 47 | DESTINATION lib/${PROJECT_NAME} 48 | ) 49 | 50 | # ----------------------------------------------- 51 | 52 | # uncomment the following section in order to fill in 53 | # further dependencies manually. 54 | # find_package( REQUIRED) 55 | 56 | if(BUILD_TESTING) 57 | find_package(ament_lint_auto REQUIRED) 58 | # the following line skips the linter which checks for copyrights 59 | # uncomment the line when a copyright and license is not present in all source files 60 | #set(ament_cmake_copyright_FOUND TRUE) 61 | # the following line skips cpplint (only works in a git repo) 62 | # uncomment the line when this package is not in a git repo 63 | #set(ament_cmake_cpplint_FOUND TRUE) 64 | ament_lint_auto_find_test_dependencies() 65 | endif() 66 | 67 | ament_package() 68 | -------------------------------------------------------------------------------- /rpi_robot_bringup/config/ps4.config.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: 0.7 9 | 10 | axis_angular: 11 | yaw: 0 12 | scale_angular: 13 | yaw: 2.5 14 | 15 | enable_button: 4 # L2 shoulder button 16 | enable_turbo_button: 5 # L1 shoulder button -------------------------------------------------------------------------------- /rpi_robot_bringup/include/rpi_robot_bringup/basic_data.hpp: -------------------------------------------------------------------------------- 1 | #ifndef BASIC_DATA_HPP 2 | #define BASIC_DATA_HPP 3 | 4 | #define BASIC_FLAG 1 5 | 6 | #endif -------------------------------------------------------------------------------- /rpi_robot_bringup/launch/lidar.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import DeclareLaunchArgument 6 | from launch.actions import IncludeLaunchDescription 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | from launch.substitutions import LaunchConfiguration 9 | from launch.substitutions import ThisLaunchFileDir 10 | from launch_ros.actions import Node 11 | 12 | 13 | def generate_launch_description(): 14 | serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0') 15 | 16 | return LaunchDescription([ 17 | DeclareLaunchArgument( 18 | 'serial_port', 19 | default_value=serial_port, 20 | description='Port for rplidar sensor' 21 | ), 22 | Node( 23 | name='rplidar_composition', 24 | package='rplidar_ros', 25 | executable='rplidar_composition', 26 | output='screen', 27 | parameters=[{ 28 | 'serial_port': serial_port, 29 | 'serial_baudrate': 115200, 30 | 'frame_id': 'laser', 31 | 'inverted': False, 32 | 'angle_compensate': True, 33 | 'scan_mode': 'Boost', 34 | }], 35 | ) 36 | ]) -------------------------------------------------------------------------------- /rpi_robot_bringup/launch/rpi_robot.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import DeclareLaunchArgument 6 | from launch.actions import IncludeLaunchDescription 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | from launch.substitutions import LaunchConfiguration 9 | from launch.substitutions import ThisLaunchFileDir 10 | from launch_ros.actions import Node 11 | 12 | 13 | def generate_launch_description(): 14 | pico_port = LaunchConfiguration('pico_port', default='/dev/ttyACM0') 15 | rplidar_port = LaunchConfiguration('rplidar_port', default='/dev/ttyUSB0') 16 | 17 | 18 | return LaunchDescription([ 19 | DeclareLaunchArgument( 20 | 'pico_port', 21 | default_value=pico_port, 22 | description='Serial port for communication with microcontroller' 23 | ), 24 | DeclareLaunchArgument( 25 | 'rplidar_port', 26 | default_value=rplidar_port, 27 | description='Port for rplidar sensor' 28 | ), 29 | # lidar launchfile 30 | IncludeLaunchDescription( 31 | PythonLaunchDescriptionSource( 32 | [ThisLaunchFileDir(), '/lidar.launch.py'] 33 | ), 34 | launch_arguments={'serial_port': rplidar_port}.items() 35 | ), 36 | # robot control node 37 | Node( 38 | package='rpi_robot_bringup', 39 | executable='rpi_robot_control.py', 40 | parameters=[{ 41 | 'pico_port': pico_port 42 | }], 43 | arguments=[], 44 | output='screen' 45 | ), 46 | # static transform for laser 47 | Node( 48 | package='tf2_ros', 49 | executable='static_transform_publisher', 50 | arguments=['-0.064', '0', '0.120', '0', '0', '0', 'base_link', 'laser'], 51 | output='screen' 52 | ), 53 | # static transform from link to footprint 54 | Node( 55 | package='tf2_ros', 56 | executable='static_transform_publisher', 57 | arguments=['0', '0', '-0.0325', '0', '0', '0', 'base_link', 'base_footprint'], 58 | output='screen' 59 | ) 60 | ]) -------------------------------------------------------------------------------- /rpi_robot_bringup/launch/rpi_robot_joy_teleop.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | import launch 6 | import launch_ros.actions 7 | 8 | 9 | def generate_launch_description(): 10 | joy_config = launch.substitutions.LaunchConfiguration('joy_config') 11 | joy_dev = launch.substitutions.LaunchConfiguration('joy_dev') 12 | config_filepath = launch.substitutions.LaunchConfiguration('config_filepath') 13 | 14 | return launch.LaunchDescription([ 15 | launch.actions.DeclareLaunchArgument('joy_config', default_value='ps4'), 16 | launch.actions.DeclareLaunchArgument('joy_dev', default_value='/dev/input/js0'), 17 | launch.actions.DeclareLaunchArgument('config_filepath', default_value=[ 18 | launch.substitutions.TextSubstitution(text=os.path.join( 19 | get_package_share_directory('rpi_robot_bringup'), 'config', '')), 20 | joy_config, launch.substitutions.TextSubstitution(text='.config.yaml')]), 21 | 22 | launch_ros.actions.Node( 23 | package='joy', node_executable='joy_node', name='joy_node', 24 | parameters=[{ 25 | 'dev': joy_dev, 26 | 'deadzone': 0.3, 27 | 'autorepeat_rate': 20.0, 28 | }]), 29 | launch_ros.actions.Node( 30 | package='teleop_twist_joy', node_executable='teleop_node', 31 | name='teleop_twist_joy_node', parameters=[config_filepath]), 32 | ]) -------------------------------------------------------------------------------- /rpi_robot_bringup/launch/rpi_robot_nostate.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import DeclareLaunchArgument 6 | from launch.actions import IncludeLaunchDescription 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | from launch.substitutions import LaunchConfiguration 9 | from launch.substitutions import ThisLaunchFileDir 10 | from launch_ros.actions import Node 11 | 12 | 13 | def generate_launch_description(): 14 | pico_port = LaunchConfiguration('pico_port', default='/dev/ttyACM0') 15 | rplidar_port = LaunchConfiguration('rplidar_port', default='/dev/ttyUSB0') 16 | 17 | 18 | return LaunchDescription([ 19 | DeclareLaunchArgument( 20 | 'pico_port', 21 | default_value=pico_port, 22 | description='Serial port for communication with microcontroller' 23 | ), 24 | DeclareLaunchArgument( 25 | 'rplidar_port', 26 | default_value=rplidar_port, 27 | description='Port for rplidar sensor' 28 | ), 29 | # lidar launchfile 30 | IncludeLaunchDescription( 31 | PythonLaunchDescriptionSource( 32 | [ThisLaunchFileDir(), '/lidar.launch.py'] 33 | ), 34 | launch_arguments={'serial_port': rplidar_port}.items() 35 | ), 36 | # robot control node 37 | Node( 38 | package='rpi_robot_bringup', 39 | executable='rpi_robot_control_nostate.py', 40 | parameters=[{ 41 | 'pico_port': pico_port 42 | }], 43 | arguments=[], 44 | output='screen' 45 | ), 46 | # static transform for laser 47 | Node( 48 | package='tf2_ros', 49 | executable='static_transform_publisher', 50 | arguments=['-0.064', '0', '0.120', '0', '0', '0', 'base_link', 'laser'], 51 | output='screen' 52 | ), 53 | # static transform from link to footprint 54 | Node( 55 | package='tf2_ros', 56 | executable='static_transform_publisher', 57 | arguments=['0', '0', '0.0325', '0', '0', '0', 'base_footprint', 'base_link'], 58 | output='screen' 59 | ) 60 | ]) -------------------------------------------------------------------------------- /rpi_robot_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rpi_robot_bringup 5 | 0.0.0 6 | TODO: Package description 7 | Jose Laruta 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | ament_cmake_python 12 | 13 | rclcpp 14 | rclpy 15 | 16 | ament_lint_auto 17 | ament_lint_common 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /rpi_robot_bringup/rpi_robot_bringup/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tabris2015/rpi-robot-ros-foxy/a9e297856d414b0550d402342a7fd51b18df320d/rpi_robot_bringup/rpi_robot_bringup/__init__.py -------------------------------------------------------------------------------- /rpi_robot_bringup/scripts/rpi_robot_control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from dataclasses import dataclass 4 | import math 5 | import time 6 | import serial 7 | import rclpy 8 | from rclpy.node import Node 9 | from rclpy.exceptions import ParameterNotDeclaredException 10 | from rcl_interfaces.msg import ParameterType 11 | 12 | from tf2_ros import TransformBroadcaster 13 | 14 | from std_msgs.msg import String 15 | from geometry_msgs.msg import Twist, Quaternion, TransformStamped 16 | from nav_msgs.msg import Odometry 17 | 18 | 19 | def quaternion_from_euler(roll, pitch, yaw) -> Quaternion: 20 | cy = math.cos(yaw * 0.5) 21 | sy = math.sin(yaw * 0.5) 22 | cp = math.cos(pitch * 0.5) 23 | sp = math.sin(pitch * 0.5) 24 | cr = math.cos(roll * 0.5) 25 | sr = math.sin(roll * 0.5) 26 | 27 | q = Quaternion() 28 | q.w = cy * cp * cr + sy * sp * sr 29 | q.x = cy * cp * sr - sy * sp * cr 30 | q.y = sy * cp * sr + cy * sp * cr 31 | q.z = sy * cp * cr - cy * sp * sr 32 | 33 | return q 34 | 35 | 36 | @dataclass 37 | class SerialStatus: 38 | """Class for different data given by the embedded system""" 39 | left_ref_speed: float 40 | right_ref_speed: float 41 | left_speed:float 42 | right_speed: float 43 | left_effort: float 44 | right_effor: float 45 | x_pos: float 46 | y_pos: float 47 | theta: float 48 | v: float 49 | w: float 50 | 51 | 52 | class RobotControlNode(Node): 53 | """Simple node for controlling a differential drive robot""" 54 | def __init__(self): 55 | super().__init__('rpi_robot_node') 56 | 57 | self.declare_parameter('pico_port', '/dev/ttyACM0') 58 | 59 | self.twist_subscription = self.create_subscription( 60 | Twist, 61 | 'cmd_vel', 62 | self.twist_callback, 63 | 10 64 | ) 65 | self.twist_subscription 66 | 67 | self.odom_publisher = self.create_publisher( 68 | Odometry, 69 | 'odom', 70 | 10 71 | ) 72 | time.sleep(0.2) 73 | self.port = self.get_parameter('pico_port').get_parameter_value().string_value 74 | self.ser = serial.Serial(self.port) 75 | self.get_logger().info(f'Using serial port {self.ser.name}') 76 | self.twist = Twist() 77 | # set timer 78 | self.pub_period = 0.04 # 0.02 seconds = 50 hz = pid rate for robot 79 | self.pub_timer = self.create_timer(self.pub_period, self.pub_callback) 80 | # tf 81 | self.tf_broadcaster = TransformBroadcaster(self) 82 | 83 | def pub_callback(self): 84 | robot_state = self.send_command(self.twist.linear.x, self.twist.angular.z) 85 | if robot_state is None: 86 | return 87 | 88 | robot_orientation = quaternion_from_euler(0, 0, robot_state.theta) 89 | timestamp = self.get_clock().now().to_msg() 90 | # transforms 91 | t = TransformStamped() 92 | t.header.stamp = timestamp 93 | t.header.frame_id = '/odom' 94 | t.child_frame_id = '/base_link' 95 | t.transform.translation.x = robot_state.x_pos 96 | t.transform.translation.y = robot_state.y_pos 97 | t.transform.translation.z = 0.0325 98 | t.transform.rotation = robot_orientation 99 | 100 | # odometry twist 101 | odom_msg = Odometry() 102 | odom_msg.header.frame_id = '/odom' 103 | odom_msg.child_frame_id = '/base_link' 104 | odom_msg.header.stamp = timestamp 105 | odom_msg.pose.pose.position.x = robot_state.x_pos 106 | odom_msg.pose.pose.position.y = robot_state.y_pos 107 | odom_msg.pose.pose.position.z = 0.325 108 | odom_msg.pose.pose.orientation = robot_orientation 109 | odom_msg.twist.twist.linear.x = robot_state.v 110 | odom_msg.twist.twist.angular.z = robot_state.w 111 | 112 | # broadcast and publish 113 | self.tf_broadcaster.sendTransform(t) 114 | self.odom_publisher.publish(odom_msg) 115 | 116 | 117 | def send_command(self, linear: float, angular: float) -> SerialStatus: 118 | self.get_logger().debug(f'Data to send: {linear}, {angular}') 119 | command = f'{linear:.3f},{angular:.3f}/'.encode('UTF-8') 120 | self.get_logger().debug(f'Sending command: "{command}"') 121 | self.ser.write(command) 122 | while self.ser.in_waiting == 0: 123 | pass 124 | 125 | res = self.ser.read(self.ser.in_waiting).decode('UTF-8') 126 | self.get_logger().debug(f'data: "{res}", bytes: {len(res)}') 127 | 128 | if res == '0' or len(res) < 79 or len(res) > (79 + 13): 129 | self.get_logger().warn(f'Bad data: "{res}"') 130 | return None 131 | 132 | raw_list = res.strip().split('/')[1].split(',') 133 | 134 | try: 135 | values_list = [float(value) for value in raw_list] 136 | except ValueError as e: 137 | self.get_logger().warn(f'Bad data: "{res}"') 138 | return None 139 | 140 | return SerialStatus(*values_list) 141 | 142 | def twist_callback(self, twist: Twist): 143 | self.twist = twist 144 | self.get_logger().info(f'Twist received: {twist}') 145 | 146 | 147 | 148 | 149 | def main(args=None): 150 | rclpy.init(args=args) 151 | robot_control_node = RobotControlNode() 152 | rclpy.spin(robot_control_node) 153 | 154 | robot_control_node.destroy_node() 155 | rclpy.shutdown() 156 | 157 | 158 | if __name__ == '__main__': 159 | main() -------------------------------------------------------------------------------- /rpi_robot_bringup/scripts/rpi_robot_control_nostate.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from dataclasses import dataclass 4 | import math 5 | import time 6 | import serial 7 | import rclpy 8 | from rclpy.node import Node 9 | from rclpy.exceptions import ParameterNotDeclaredException 10 | from rcl_interfaces.msg import ParameterType 11 | 12 | from tf2_ros import TransformBroadcaster 13 | 14 | from std_msgs.msg import String 15 | from geometry_msgs.msg import Twist, Quaternion, TransformStamped 16 | from nav_msgs.msg import Odometry 17 | 18 | 19 | def quaternion_from_euler(roll, pitch, yaw) -> Quaternion: 20 | cy = math.cos(yaw * 0.5) 21 | sy = math.sin(yaw * 0.5) 22 | cp = math.cos(pitch * 0.5) 23 | sp = math.sin(pitch * 0.5) 24 | cr = math.cos(roll * 0.5) 25 | sr = math.sin(roll * 0.5) 26 | 27 | q = Quaternion() 28 | q.w = cy * cp * cr + sy * sp * sr 29 | q.x = cy * cp * sr - sy * sp * cr 30 | q.y = sy * cp * sr + cy * sp * cr 31 | q.z = sy * cp * cr - cy * sp * sr 32 | 33 | return q 34 | 35 | 36 | @dataclass 37 | class SerialStatus: 38 | """Class for different data given by the embedded system""" 39 | left_ref_speed: float 40 | right_ref_speed: float 41 | left_speed:float 42 | right_speed: float 43 | left_effort: float 44 | right_effor: float 45 | x_pos: float 46 | y_pos: float 47 | theta: float 48 | v: float 49 | w: float 50 | 51 | 52 | class RobotControlNode(Node): 53 | """Simple node for controlling a differential drive robot""" 54 | def __init__(self): 55 | super().__init__('rpi_robot_node') 56 | 57 | self.declare_parameter('pico_port', '/dev/ttyACM0') 58 | 59 | self.twist_subscription = self.create_subscription( 60 | Twist, 61 | 'cmd_vel', 62 | self.twist_callback, 63 | 10 64 | ) 65 | self.twist_subscription 66 | 67 | self.odom_publisher = self.create_publisher( 68 | Odometry, 69 | 'odom', 70 | 10 71 | ) 72 | time.sleep(0.2) 73 | self.port = self.get_parameter('pico_port').get_parameter_value().string_value 74 | self.ser = serial.Serial(self.port) 75 | self.get_logger().info(f'Using serial port {self.ser.name}') 76 | self.twist = Twist() 77 | # set timer 78 | self.pub_period = 0.04 # 0.02 seconds = 50 hz = pid rate for robot 79 | self.pub_timer = self.create_timer(self.pub_period, self.timer_callback) 80 | 81 | def timer_callback(self): 82 | robot_state = self.send_command(self.twist.linear.x, self.twist.angular.z) 83 | if robot_state is None: 84 | return 85 | 86 | 87 | def send_command(self, linear: float, angular: float) -> SerialStatus: 88 | self.get_logger().debug(f'Data to send: {linear}, {angular}') 89 | command = f'{linear:.3f},{angular:.3f}/'.encode('UTF-8') 90 | self.get_logger().debug(f'Sending command: "{command}"') 91 | self.ser.write(command) 92 | while self.ser.in_waiting == 0: 93 | pass 94 | 95 | res = self.ser.read(self.ser.in_waiting).decode('UTF-8') 96 | self.get_logger().debug(f'data: "{res}", bytes: {len(res)}') 97 | 98 | if res == '0' or len(res) < 79 or len(res) > (79 + 13): 99 | self.get_logger().warn(f'Bad data: "{res}"') 100 | return None 101 | 102 | raw_list = res.strip().split('/')[1].split(',') 103 | 104 | try: 105 | values_list = [float(value) for value in raw_list] 106 | except ValueError as e: 107 | self.get_logger().warn(f'Bad data: "{res}"') 108 | return None 109 | 110 | return SerialStatus(*values_list) 111 | 112 | def twist_callback(self, twist: Twist): 113 | self.twist = twist 114 | self.get_logger().info(f'Twist received: {twist}') 115 | 116 | 117 | 118 | 119 | def main(args=None): 120 | rclpy.init(args=args) 121 | robot_control_node = RobotControlNode() 122 | rclpy.spin(robot_control_node) 123 | 124 | robot_control_node.destroy_node() 125 | rclpy.shutdown() 126 | 127 | 128 | if __name__ == '__main__': 129 | main() -------------------------------------------------------------------------------- /rpi_robot_bringup/scripts/rpi_robot_echo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rclpy 4 | from rclpy.node import Node 5 | 6 | from std_msgs.msg import String 7 | 8 | 9 | class EchoNode(Node): 10 | """Simple node for echoing an incoming message""" 11 | def __init__(self): 12 | super().__init__('robot_node') 13 | self.subscription = self.create_subscription( 14 | String, 15 | 'rpi_msg', 16 | self.string_callback, 17 | 10 18 | ) 19 | self.subscription 20 | 21 | self.str_publisher = self.create_publisher( 22 | String, 23 | 'rpi_echo', 24 | 10 25 | ) 26 | 27 | def string_callback(self, string: String): 28 | msg = String() 29 | self.get_logger().info(f'Received: {string.data}') 30 | msg.data = f'echo: {string.data}' 31 | 32 | 33 | def main(args=None): 34 | rclpy.init(args=args) 35 | echo_node = EchoNode() 36 | rclpy.spin(echo_node) 37 | 38 | echo_node.destroy_node() 39 | rclpy.shutdown() 40 | 41 | 42 | if __name__ == '__main__': 43 | main() -------------------------------------------------------------------------------- /rpi_robot_bringup/src/basic_node.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | // #include "rpi_robot_bringup/basic_data.hpp" 3 | 4 | int main(int argc, char **argv) 5 | { 6 | rclcpp::init(argc, argv); 7 | auto node = std::make_shared("basic_node"); 8 | rclcpp::spin(node); 9 | rclcpp::shutdown(); 10 | return 0; 11 | } -------------------------------------------------------------------------------- /rpi_robot_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rpi_robot_control 5 | 0.0.0 6 | TODO: Package description 7 | pepe 8 | TODO: License declaration 9 | 10 | rclpy 11 | std_msgs 12 | nav_msgs 13 | geometry_msgs 14 | 15 | ament_copyright 16 | ament_flake8 17 | ament_pep257 18 | python3-pytest 19 | 20 | 21 | ament_python 22 | 23 | 24 | -------------------------------------------------------------------------------- /rpi_robot_control/resource/rpi_robot_control: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tabris2015/rpi-robot-ros-foxy/a9e297856d414b0550d402342a7fd51b18df320d/rpi_robot_control/resource/rpi_robot_control -------------------------------------------------------------------------------- /rpi_robot_control/rpi_robot_control/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tabris2015/rpi-robot-ros-foxy/a9e297856d414b0550d402342a7fd51b18df320d/rpi_robot_control/rpi_robot_control/__init__.py -------------------------------------------------------------------------------- /rpi_robot_control/rpi_robot_control/go_to_goal.py: -------------------------------------------------------------------------------- 1 | from os import times 2 | import rclpy 3 | import time 4 | import math 5 | import csv 6 | from rclpy.node import Node 7 | from rclpy.action import ActionServer, CancelResponse, GoalResponse 8 | from rclpy.callback_groups import ReentrantCallbackGroup 9 | from rclpy.executors import MultiThreadedExecutor 10 | from tf2_ros import TransformBroadcaster 11 | 12 | from nav_msgs.msg import Odometry 13 | from geometry_msgs.msg import Twist, Quaternion, TransformStamped 14 | from sensor_msgs.msg import LaserScan 15 | 16 | from rpi_robot_action_interfaces.action import GoToGoal 17 | from rpi_robot_control.pid import PidController 18 | from rpi_robot_control.lidar import Lidar 19 | 20 | 21 | 22 | 23 | class GoToGoalNode(Node): 24 | """Node for high level control of a differential drive robot""" 25 | def __init__(self): 26 | 27 | super().__init__('go_to_goal_node') 28 | self.get_logger().info("creating publisher...") 29 | self.twist_publisher = self.create_publisher( 30 | Twist, 31 | 'cmd_vel', 32 | 10 33 | ) 34 | 35 | self.get_logger().info("creating odometry subscriber...") 36 | self.odom_subscription = self.create_subscription( 37 | Odometry, 38 | 'odom', 39 | self.odom_callback, 40 | 10 41 | ) 42 | self.odom_subscription 43 | 44 | self.get_logger().info("creating laser subscriber...") 45 | self.lidar_subscription = self.create_subscription( 46 | LaserScan, 47 | 'scan', 48 | self.lidar_callback, 49 | 10 50 | ) 51 | self.lidar_subscription 52 | self.lidar = Lidar(self) 53 | 54 | self.get_logger().info("creating action server...") 55 | self.go_to_goal_action_service = ActionServer( 56 | self, 57 | GoToGoal, 58 | 'go_to_goal_service', 59 | execute_callback=self.execute_callback, 60 | callback_group=ReentrantCallbackGroup(), 61 | goal_callback=self.goal_callback, 62 | cancel_callback=self.cancel_callback 63 | ) 64 | 65 | # transform 66 | self.tf_broadcaster = TransformBroadcaster(self) 67 | # important variables 68 | self.is_moving = False 69 | self.pos_tolerance = 0.05 70 | self.goal_x = 0 71 | self.goal_y = 0 72 | self.current_x = 0 73 | self.current_y = 0 74 | 75 | self.gtg_r = 0 76 | self.gtg_theta = 0 77 | self.ao_r = 0 78 | self.ao_theta = 0 79 | self.theta_desired = 0 80 | self.r_desired = 0 81 | 82 | self.sample_time = 0.1 83 | self.max_linear_v = 0.08 84 | self.alpha = 5 85 | self.blend_dist_threshold = 0.4 # m 86 | self.avoid_dist_threshold = 0.2 87 | self.avoid_angle_threshold = math.pi / 2.0 88 | self.angle_pid = PidController(0.3, 0.01, 0.002, self.sample_time, True) 89 | self.angle_pid.set_output_limits(-3, 3) 90 | 91 | self.get_logger().info("initialization finished") 92 | 93 | self.linear_command = 0 94 | self.angular_command = 0 95 | 96 | ## logging utils 97 | 98 | 99 | def odom_callback(self, odom: Odometry): 100 | self.current_x = odom.pose.pose.position.x 101 | self.current_y = odom.pose.pose.position.y 102 | self.current_theta = self.get_euler_from_quaternion(odom.pose.pose.orientation)[2] 103 | # self.get_logger().info(f'Current Position: ({self.current_x}, {self.current_y}, {self.current_theta})') 104 | # get direction vectors for both behaviors 105 | # self.get_logger().info('get "go-to-goal" vector') 106 | self.gtg_r, self.gtg_theta = self.get_go_to_goal_vector() 107 | # self.get_logger().info('get "avoid obstacle" vector') 108 | self.ao_r, self.ao_theta = self.get_obstacle_vector() 109 | 110 | self.publish_ref_vectors() 111 | 112 | 113 | def lidar_callback(self, scan: LaserScan): 114 | self.lidar.update(scan) 115 | 116 | def add_two_callback(self, request, response): 117 | response.sum = request.a + request.b 118 | self.get_logger().info(f'Incoming request a:{request.a}, b:{request.b}') 119 | return response 120 | 121 | def goal_callback(self, goal_request): 122 | """Accept or reject a client request to begin an action""" 123 | if self.is_moving: 124 | return GoalResponse.REJECT 125 | 126 | self.goal_x = goal_request.x 127 | self.goal_y = goal_request.y 128 | self.get_logger().info(f'Received goal request: ({self.goal_x}, {self.goal_y})') 129 | self.get_logger().info(f'Distance to goal: {self.get_distance_to_goal()}') 130 | 131 | return GoalResponse.ACCEPT 132 | 133 | def cancel_callback(self, goal_handle): 134 | """Accept or reject a client request to cancel an action""" 135 | self.get_logger().info('Received cancel request') 136 | # TODO: implement logic to cancel request 137 | return CancelResponse.ACCEPT 138 | 139 | # async method 140 | async def execute_callback(self, goal_handle): 141 | """Execute a goal""" 142 | self.get_logger().info('Executing action...') 143 | 144 | feedback_msg = GoToGoal.Feedback() 145 | log_file = open(f'/home/pepe/log/{int(time.time() * 1000)}.log', 'w') 146 | 147 | while not self.update_control_loop(): 148 | self.is_moving = True 149 | feedback_msg.current_x = self.current_x 150 | feedback_msg.current_y = self.current_x 151 | feedback_msg.distance = self.get_distance_to_goal() 152 | self.get_logger().info(f'current pos: {feedback_msg.current_x},{feedback_msg.current_y}, distance to goal: {self.get_distance_to_goal()}') 153 | self.get_logger().info(f'go_to_goal: [{self.gtg_r:.2f},{self.gtg_theta:.2f}], avoid obstacle: [{self.ao_r:.2f},{self.ao_theta:.2f}], desired: [{self.r_desired:.2f},{self.theta_desired:.2f}]') 154 | self.send_twist_command() 155 | goal_handle.publish_feedback(feedback_msg) 156 | log_str = f'{int(time.time() * 1000)},{self.current_x},{self.current_y},{self.get_distance_to_goal()},{self.linear_command},{self.angular_command}\n' 157 | log_file.write(log_str) 158 | time.sleep(self.sample_time) 159 | 160 | self.is_moving = False 161 | # should be 0 162 | self.send_twist_command() 163 | log_str = f'{int(time.time() * 1000)},{self.current_x},{self.current_y},{self.get_distance_to_goal()},{self.linear_command},{self.angular_command}\n' 164 | log_file.write(log_str) 165 | log_file.close() 166 | goal_handle.succeed() 167 | result = GoToGoal.Result() 168 | result.goal_reached = True 169 | self.get_logger().info(f'Returning result: {result.goal_reached}') 170 | return result 171 | 172 | 173 | def update_control_loop(self): 174 | 175 | 176 | # TODO: blending 177 | self.r_desired = self.gtg_r 178 | # arctan2(r2sin(ϕ2−ϕ1),r1+r2cos(ϕ2−ϕ1)) 179 | delta_theta = self.gtg_theta - self.ao_theta 180 | delta_theta = math.atan2(math.sin(delta_theta), math.cos(delta_theta)) 181 | 182 | 183 | if self.ao_r <= self.avoid_dist_threshold: 184 | # too close, avoid 185 | avoid_theta = self.ao_theta + math.pi 186 | avoid_theta = math.atan2(math.sin(avoid_theta), math.cos(avoid_theta)) 187 | self.theta_desired = avoid_theta 188 | 189 | elif self.ao_r > self.avoid_dist_threshold and self.ao_r <= self.blend_dist_threshold and abs(delta_theta) < self.avoid_angle_threshold: 190 | # blend perpendicular 191 | avoid_theta = self.ao_theta + (math.copysign(1, delta_theta)) * (math.pi / 2.0) 192 | avoid_theta = math.atan2(math.sin(avoid_theta), math.cos(avoid_theta)) 193 | self.theta_desired = self.gtg_theta + math.atan2(self.ao_r * math.sin(self.ao_theta - self.gtg_theta), self.gtg_r + self.ao_r * math.cos(self.ao_theta - self.gtg_theta)) 194 | 195 | else: 196 | self.theta_desired = self.gtg_theta 197 | 198 | #------- 199 | 200 | 201 | if self.get_distance_to_goal() <= self.pos_tolerance: 202 | self.linear_command = 0 203 | self.angular_command = 0 204 | self.get_logger().info(f"Already near goal, distance: {self.get_distance_to_goal()}") 205 | return True # goal reached! 206 | 207 | # input to angle pid 208 | self.angle_pid.set_input(self.current_theta) 209 | # setpoint 210 | self.angle_pid.set_setpoint(self.theta_desired) 211 | 212 | # compute pid 213 | self.angle_pid.compute() 214 | 215 | self.linear_command = self.r_desired 216 | self.angular_command = self.angle_pid.get_output() # from pid 217 | 218 | return False 219 | 220 | 221 | def get_go_to_goal_vector(self): 222 | error_x = self.goal_x - self.current_x 223 | error_y = self.goal_y - self.current_y 224 | # self.get_logger().info(f'gtg error: ({error_x}, {error_y})') 225 | # compute K for linear velocity 226 | distance_to_goal = self.get_distance_to_goal() 227 | # self.get_logger().info(f'gtg distance: {distance_to_goal}') 228 | 229 | K = (self.max_linear_v * (1 - math.exp(-self.alpha * (distance_to_goal * distance_to_goal))) / (distance_to_goal * distance_to_goal)) if distance_to_goal > 0.0001 else 0 230 | 231 | # compute control signal 232 | u_x = K * error_x 233 | u_y = K * error_y 234 | 235 | # convert to polar coordinates 236 | r = math.sqrt((u_x * u_x) + (u_y * u_y)) 237 | theta = math.atan2(u_y, u_x) 238 | return r, theta 239 | 240 | def get_obstacle_vector(self): 241 | r_obs, theta_obs = self.lidar.get_closest_obstacle() 242 | return r_obs, theta_obs 243 | 244 | def get_distance_to_goal(self): 245 | return math.sqrt(math.pow(self.goal_x - self.current_x, 2) + math.pow(self.goal_y - self.current_y, 2)) 246 | 247 | def get_euler_from_quaternion(self, quaternion): 248 | x = quaternion.x 249 | y = quaternion.y 250 | z = quaternion.z 251 | w = quaternion.w 252 | 253 | sinr_cosp = 2 * (w * x + y * z) 254 | cosr_cosp = 1 - 2 * (x * x + y * y) 255 | roll = math.atan2(sinr_cosp, cosr_cosp) 256 | 257 | sinp = 2 * (w * y - z * x) 258 | pitch = math.asin(sinp) 259 | 260 | siny_cosp = 2 * (w * z + x * y) 261 | cosy_cosp = 1 - 2 * (y * y + z * z) 262 | yaw = math.atan2(siny_cosp, cosy_cosp) 263 | 264 | return roll, pitch, yaw 265 | 266 | def quaternion_from_euler(self, roll, pitch, yaw) -> Quaternion: 267 | cy = math.cos(yaw * 0.5) 268 | sy = math.sin(yaw * 0.5) 269 | cp = math.cos(pitch * 0.5) 270 | sp = math.sin(pitch * 0.5) 271 | cr = math.cos(roll * 0.5) 272 | sr = math.sin(roll * 0.5) 273 | 274 | q = Quaternion() 275 | q.w = cy * cp * cr + sy * sp * sr 276 | q.x = cy * cp * sr - sy * sp * cr 277 | q.y = sy * cp * sr + cy * sp * cr 278 | q.z = sy * cp * cr - cy * sp * sr 279 | 280 | return q 281 | 282 | def send_twist_command(self): 283 | twist_msg = Twist() 284 | twist_msg.linear.x = float(self.linear_command) 285 | twist_msg.angular.z = float(self.angular_command) 286 | self.get_logger().info(f'Publishing twist: {twist_msg}') 287 | self.twist_publisher.publish(twist_msg) 288 | 289 | def publish_ref_vectors(self): 290 | timestamp = self.get_clock().now().to_msg() 291 | t_goal = TransformStamped() 292 | t_goal.header.stamp = timestamp 293 | t_goal.header.frame_id = '/base_link' 294 | t_goal.child_frame_id = '/gtg' 295 | 296 | t_goal.transform.rotation = self.quaternion_from_euler(0, 0, self.gtg_theta) 297 | 298 | t_obstacle = TransformStamped() 299 | t_obstacle.header.stamp = timestamp 300 | t_obstacle.header.frame_id = '/base_link' 301 | t_obstacle.child_frame_id = '/ao' 302 | 303 | t_obstacle.transform.rotation = self.quaternion_from_euler(0, 0, self.ao_theta) 304 | 305 | t_desired = TransformStamped() 306 | t_desired.header.stamp = timestamp 307 | t_desired.header.frame_id = '/base_link' 308 | t_desired.child_frame_id = '/desired' 309 | 310 | t_desired.transform.rotation = self.quaternion_from_euler(0, 0, self.theta_desired) 311 | # self.get_logger().info('Publishing transforms for vectors') 312 | self.tf_broadcaster.sendTransform(t_goal) 313 | self.tf_broadcaster.sendTransform(t_obstacle) 314 | self.tf_broadcaster.sendTransform(t_desired) 315 | 316 | 317 | def main(args=None): 318 | rclpy.init(args=args) 319 | go_to_goal_node = GoToGoalNode() 320 | 321 | executor = MultiThreadedExecutor() 322 | 323 | rclpy.spin(go_to_goal_node, executor=executor) 324 | 325 | go_to_goal_node.destroy_node() 326 | rclpy.shutdown() 327 | 328 | 329 | if __name__ == '__main__': 330 | main() -------------------------------------------------------------------------------- /rpi_robot_control/rpi_robot_control/lidar.py: -------------------------------------------------------------------------------- 1 | from numpy.core.numeric import Inf 2 | from rclpy.node import Node 3 | from sensor_msgs.msg import LaserScan 4 | import numpy as np 5 | import math 6 | 7 | class Lidar: 8 | def __init__(self, node: Node): 9 | self.angles = np.array([]) 10 | self.distances = np.array([]) 11 | 12 | self.theta_obstacle = 0 13 | self.r_obstacle = 0 14 | self.node = node 15 | self.LOGGER = self.node.get_logger() 16 | 17 | 18 | def update(self, scan: LaserScan): 19 | # self.LOGGER.info(f'Laser: [{scan.angle_min:.3f},{scan.angle_max:.3f},{scan.angle_increment:.3f}] ranges: {len(scan.ranges)}') 20 | 21 | self.angles = np.arange(scan.angle_min, scan.angle_max, scan.angle_increment)# * (180.0 / math.pi) 22 | self.distances = np.array(scan.ranges) 23 | # filter inf numbers 24 | if len(self.distances) != len(self.angles): 25 | return 26 | 27 | # self.LOGGER.info(f'angles: {len(self.angles)}, distances: {len(self.distances)}') 28 | # self.LOGGER.info(f'min distance: ({self.angles[np.argmin(self.distances)]}deg,{np.min(self.distances)}m) - id: {np.argmin(self.distances)}') 29 | 30 | # angle and distance relative to robot 31 | self.theta_obstacle = self.angles[np.argmin(self.distances)] 32 | self.r_obstacle = np.min(self.distances) 33 | 34 | def get_closest_obstacle(self): 35 | # self.LOGGER.info(f'obstacle: {self.theta_obstacle * (180.0 / math.pi)} [deg] {self.r_obstacle} [m]') 36 | return self.r_obstacle, self.theta_obstacle 37 | 38 | -------------------------------------------------------------------------------- /rpi_robot_control/rpi_robot_control/pid.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | 4 | class PidController: 5 | def __init__(self, kp: float, ki: float, kd: float, sample_time:float, is_angle: bool): 6 | self.kp = kp 7 | self.ki = ki 8 | self.kd = kd 9 | self.sample_time = sample_time 10 | self.is_angle = is_angle 11 | self.out_min = 0 12 | self.out_max = 1 13 | self.output = 0 14 | self.out_sum = 0 15 | self.input = 0 16 | self.setpoint = 0 17 | self.error = 0 18 | self.last_input = 0 19 | self.set_output_limits(-1.0, 1.0) 20 | self.set_gains(self.kp, self.ki, self.kd) 21 | 22 | def set_sample_time(self, new_sample_time): 23 | if new_sample_time <= 0.0: 24 | return 25 | 26 | ratio = new_sample_time / self.sample_time 27 | self.ki *= ratio 28 | self.kd /= ratio 29 | self.sample_time = new_sample_time 30 | 31 | def set_gains(self, kp, ki, kd): 32 | if kp < 0 or ki < 0 or kd < 0: 33 | return 34 | 35 | self.kp = kp 36 | self.ki = ki * self.sample_time 37 | self.kd = kd / self.sample_time 38 | 39 | def set_output_limits(self, min, max): 40 | if min >= max: 41 | return 42 | 43 | self.out_min = min 44 | self.out_max = max 45 | 46 | if self.output > self.out_max: 47 | self.output = self.out_max 48 | elif self.output < self.out_min: 49 | self.output = self.out_min 50 | 51 | if self.out_sum > self.out_max: 52 | self.out_sum = self.out_max 53 | elif self.out_sum < self.out_min: 54 | self.out_sum = self.out_min 55 | 56 | def compute(self): 57 | self.error = self.setpoint - self.input 58 | if self.is_angle: 59 | self.error = math.atan2(math.sin(self.error), math.cos(self.error)) 60 | 61 | delta_input = self.input - self.last_input 62 | self.out_sum += self.ki * self.error 63 | 64 | if self.out_sum > self.out_max: 65 | self.out_sum = self.out_max 66 | elif self.out_sum < self.out_min: 67 | self.out_sum = self.out_min 68 | 69 | self.output = self.kp * self.error + self.out_sum - self.kd * delta_input 70 | 71 | if self.output > self.out_max: 72 | self.output = self.out_max 73 | elif self.output < self.out_min: 74 | self.output = self.out_min 75 | 76 | self.last_input = self.input 77 | 78 | def set_input(self, new_input): 79 | self.input = new_input 80 | 81 | def set_setpoint(self, new_setpoint): 82 | self.setpoint = new_setpoint 83 | 84 | def get_output(self): 85 | return self.output -------------------------------------------------------------------------------- /rpi_robot_control/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script-dir=$base/lib/rpi_robot_control 3 | [install] 4 | install-scripts=$base/lib/rpi_robot_control 5 | -------------------------------------------------------------------------------- /rpi_robot_control/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | package_name = 'rpi_robot_control' 4 | 5 | setup( 6 | name=package_name, 7 | version='0.0.0', 8 | packages=[package_name], 9 | data_files=[ 10 | ('share/ament_index/resource_index/packages', 11 | ['resource/' + package_name]), 12 | ('share/' + package_name, ['package.xml']), 13 | ], 14 | install_requires=['setuptools'], 15 | zip_safe=True, 16 | maintainer='pepe', 17 | maintainer_email='eduardo.laruta@gmail.com', 18 | description='TODO: Package description', 19 | license='TODO: License declaration', 20 | tests_require=['pytest'], 21 | entry_points={ 22 | 'console_scripts': [ 23 | 'go_to_goal = rpi_robot_control.go_to_goal:main', 24 | ], 25 | }, 26 | ) 27 | -------------------------------------------------------------------------------- /rpi_robot_control/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 | -------------------------------------------------------------------------------- /rpi_robot_control/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, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /rpi_robot_control/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 | -------------------------------------------------------------------------------- /rpi_robot_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(rpi_robot_slam) 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 | # uncomment the following section in order to fill in 21 | # further dependencies manually. 22 | # find_package( REQUIRED) 23 | 24 | install( 25 | DIRECTORY launch config 26 | DESTINATION share/${PROJECT_NAME} 27 | ) 28 | 29 | 30 | if(BUILD_TESTING) 31 | find_package(ament_lint_auto REQUIRED) 32 | # the following line skips the linter which checks for copyrights 33 | # uncomment the line when a copyright and license is not present in all source files 34 | #set(ament_cmake_copyright_FOUND TRUE) 35 | # the following line skips cpplint (only works in a git repo) 36 | # uncomment the line when this package is not in a git repo 37 | #set(ament_cmake_cpplint_FOUND TRUE) 38 | ament_lint_auto_find_test_dependencies() 39 | endif() 40 | 41 | ament_package() 42 | -------------------------------------------------------------------------------- /rpi_robot_slam/config/rpi_robot_lds_2d.lua: -------------------------------------------------------------------------------- 1 | -- Copyright 2016 The Cartographer Authors 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 | -- /* Author: Darby Lim */ 16 | 17 | include "map_builder.lua" 18 | include "trajectory_builder.lua" 19 | 20 | options = { 21 | map_builder = MAP_BUILDER, 22 | trajectory_builder = TRAJECTORY_BUILDER, 23 | map_frame = "map", 24 | tracking_frame = "base_link", 25 | published_frame = "base_link", 26 | odom_frame = "odom", 27 | provide_odom_frame = false, 28 | publish_frame_projected_to_2d = true, 29 | use_odometry = false, 30 | use_nav_sat = false, 31 | use_landmarks = false, 32 | num_laser_scans = 1, 33 | num_multi_echo_laser_scans = 0, 34 | num_subdivisions_per_laser_scan = 1, 35 | num_point_clouds = 0, 36 | lookup_transform_timeout_sec = 0.2, 37 | submap_publish_period_sec = 0.3, 38 | pose_publish_period_sec = 5e-3, 39 | trajectory_publish_period_sec = 30e-3, 40 | rangefinder_sampling_ratio = 1., 41 | odometry_sampling_ratio = 1., 42 | fixed_frame_pose_sampling_ratio = 1., 43 | imu_sampling_ratio = 1., 44 | landmarks_sampling_ratio = 1., 45 | } 46 | 47 | MAP_BUILDER.use_trajectory_builder_2d = true 48 | 49 | TRAJECTORY_BUILDER_2D.min_range = 0.12 50 | TRAJECTORY_BUILDER_2D.max_range = 10 51 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 9.5 52 | TRAJECTORY_BUILDER_2D.use_imu_data = false 53 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 54 | TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) 55 | 56 | POSE_GRAPH.constraint_builder.min_score = 0.65 57 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7 58 | 59 | -- POSE_GRAPH.optimize_every_n_nodes = 0 60 | 61 | return options 62 | -------------------------------------------------------------------------------- /rpi_robot_slam/launch/cartographer.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 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 | # Author: Darby Lim 16 | 17 | import os 18 | from ament_index_python.packages import get_package_share_directory 19 | from launch import LaunchDescription 20 | from launch.actions import DeclareLaunchArgument 21 | from launch_ros.actions import Node 22 | from launch.substitutions import LaunchConfiguration 23 | from launch.actions import IncludeLaunchDescription 24 | from launch.launch_description_sources import PythonLaunchDescriptionSource 25 | from launch.substitutions import ThisLaunchFileDir 26 | 27 | 28 | def generate_launch_description(): 29 | use_sim_time = LaunchConfiguration('use_sim_time', default='false') 30 | rpi_robot_prefix = get_package_share_directory('rpi_robot_slam') 31 | cartographer_config_dir = LaunchConfiguration('cartographer_config_dir', default=os.path.join( 32 | rpi_robot_prefix, 'config')) 33 | configuration_basename = LaunchConfiguration('configuration_basename', 34 | default='rpi_robot_lds_2d.lua') 35 | 36 | resolution = LaunchConfiguration('resolution', default='0.05') 37 | publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0') 38 | 39 | return LaunchDescription([ 40 | DeclareLaunchArgument( 41 | 'cartographer_config_dir', 42 | default_value=cartographer_config_dir, 43 | description='Full path to config file to load'), 44 | DeclareLaunchArgument( 45 | 'configuration_basename', 46 | default_value=configuration_basename, 47 | description='Name of lua file for cartographer'), 48 | DeclareLaunchArgument( 49 | 'use_sim_time', 50 | default_value='false', 51 | description='Use simulation (Gazebo) clock if true'), 52 | 53 | Node( 54 | package='cartographer_ros', 55 | executable='cartographer_node', 56 | name='cartographer_node', 57 | output='screen', 58 | parameters=[{'use_sim_time': use_sim_time}], 59 | arguments=['-configuration_directory', cartographer_config_dir, 60 | '-configuration_basename', configuration_basename]), 61 | 62 | DeclareLaunchArgument( 63 | 'resolution', 64 | default_value=resolution, 65 | description='Resolution of a grid cell in the published occupancy grid'), 66 | 67 | DeclareLaunchArgument( 68 | 'publish_period_sec', 69 | default_value=publish_period_sec, 70 | description='OccupancyGrid publishing period'), 71 | 72 | IncludeLaunchDescription( 73 | PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/occupancy_grid.launch.py']), 74 | launch_arguments={'use_sim_time': use_sim_time, 'resolution': resolution, 75 | 'publish_period_sec': publish_period_sec}.items(), 76 | ) 77 | ]) 78 | -------------------------------------------------------------------------------- /rpi_robot_slam/launch/occupancy_grid.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 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 | # Author: Darby Lim 16 | 17 | from launch import LaunchDescription 18 | from launch.actions import DeclareLaunchArgument 19 | from launch_ros.actions import Node 20 | from launch.substitutions import LaunchConfiguration 21 | 22 | 23 | def generate_launch_description(): 24 | use_sim_time = LaunchConfiguration('use_sim_time', default='false') 25 | resolution = LaunchConfiguration('resolution', default='0.05') 26 | publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0') 27 | 28 | return LaunchDescription([ 29 | DeclareLaunchArgument( 30 | 'resolution', 31 | default_value=resolution, 32 | description='Resolution of a grid cell in the published occupancy grid'), 33 | 34 | DeclareLaunchArgument( 35 | 'publish_period_sec', 36 | default_value=publish_period_sec, 37 | description='OccupancyGrid publishing period'), 38 | 39 | DeclareLaunchArgument( 40 | 'use_sim_time', 41 | default_value='false', 42 | description='Use simulation (Gazebo) clock if true'), 43 | 44 | Node( 45 | package='cartographer_ros', 46 | executable='occupancy_grid_node', 47 | name='occupancy_grid_node', 48 | output='screen', 49 | parameters=[{'use_sim_time': use_sim_time}], 50 | arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec]), 51 | ]) 52 | -------------------------------------------------------------------------------- /rpi_robot_slam/launch/rpi_robot_slam.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2019 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 | # Author: Darby Lim 16 | 17 | import os 18 | from ament_index_python.packages import get_package_share_directory 19 | from launch import LaunchDescription 20 | from launch.actions import DeclareLaunchArgument 21 | from launch_ros.actions import Node 22 | from launch.substitutions import LaunchConfiguration 23 | from launch.actions import IncludeLaunchDescription 24 | from launch.launch_description_sources import PythonLaunchDescriptionSource 25 | from launch.substitutions import ThisLaunchFileDir 26 | 27 | 28 | def generate_launch_description(): 29 | rpi_robot_bringup_prefix = get_package_share_directory('rpi_robot_bringup') 30 | return LaunchDescription([ 31 | IncludeLaunchDescription( 32 | PythonLaunchDescriptionSource([rpi_robot_bringup_prefix, '/launch', '/rpi_robot_nostate.launch.py']), 33 | launch_arguments={}.items(), 34 | ), 35 | IncludeLaunchDescription( 36 | PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/cartographer.launch.py']) 37 | ) 38 | ]) 39 | -------------------------------------------------------------------------------- /rpi_robot_slam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rpi_robot_slam 5 | 0.0.0 6 | TODO: Package description 7 | pepe 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | cartographer_ros 13 | 14 | ament_lint_auto 15 | ament_lint_common 16 | 17 | 18 | ament_cmake 19 | 20 | 21 | --------------------------------------------------------------------------------