Copy disabled (too large)
Download .txt
Showing preview only (23,001K chars total). Download the full file to get everything.
Repository: TheNoobInventor/lidarbot
Branch: main
Commit: 89f8ec0dfed8
Files: 131
Total size: 21.9 MB
Directory structure:
gitextract_xh4fad55/
├── .github/
│ └── workflows/
│ └── lidarbot_ci_action.yml
├── .gitignore
├── LICENSE
├── README.md
├── docs/
│ └── index.md
├── lidarbot/
│ ├── CMakeLists.txt
│ └── package.xml
├── lidarbot_aruco/
│ ├── config/
│ │ ├── chessboard_calibration.yaml
│ │ ├── params_1.yaml
│ │ └── params_2.yaml
│ ├── launch/
│ │ └── trajectory_visualizer_launch.py
│ ├── lidarbot_aruco/
│ │ ├── __init__.py
│ │ ├── aruco_trajectory_visualizer.py
│ │ ├── detect_aruco_marker.py
│ │ └── generate_aruco_marker.py
│ ├── package.xml
│ ├── resource/
│ │ └── lidarbot_aruco
│ ├── setup.cfg
│ ├── setup.py
│ └── test/
│ ├── test_copyright.py
│ ├── test_flake8.py
│ └── test_pep257.py
├── lidarbot_base/
│ ├── CMakeLists.txt
│ ├── include/
│ │ └── lidarbot_base/
│ │ ├── DEV_Config.h
│ │ ├── Debug.h
│ │ ├── MotorDriver.h
│ │ ├── PCA9685.h
│ │ ├── lidarbot_hardware.hpp
│ │ ├── motor_encoder.h
│ │ └── wheel.hpp
│ ├── lidarbot_hardware.xml
│ ├── package.xml
│ └── src/
│ ├── DEV_Config.c
│ ├── MotorDriver.c
│ ├── PCA9685.c
│ ├── lidarbot_hardware.cpp
│ ├── motor_checks_client.cpp
│ ├── motor_checks_server.cpp
│ ├── motor_encoder.c
│ └── wheel.cpp
├── lidarbot_bringup/
│ ├── CMakeLists.txt
│ ├── LICENSE
│ ├── config/
│ │ ├── controllers.yaml
│ │ └── gazebo_ros2_ctl_use_sim.yaml
│ ├── include/
│ │ └── lidarbot_bringup/
│ │ ├── mpu6050_hardware_interface.hpp
│ │ └── mpu6050_lib.h
│ ├── launch/
│ │ ├── camera_launch.py
│ │ ├── lidarbot_bringup_launch.py
│ │ └── rplidar_launch.py
│ ├── mpu6050_hardware.xml
│ ├── package.xml
│ └── src/
│ ├── mpu6050_covariances.cpp
│ ├── mpu6050_hardware_interface.cpp
│ ├── mpu6050_lib.cpp
│ └── mpu6050_offsets.cpp
├── lidarbot_description/
│ ├── CMakeLists.txt
│ ├── launch/
│ │ ├── description_launch.py
│ │ └── robot_state_publisher_launch.py
│ ├── meshes/
│ │ ├── MPU6050.dae
│ │ ├── caster_mount.dae
│ │ ├── caster_wheel.dae
│ │ ├── power_bank.dae
│ │ ├── power_bank_stand.dae
│ │ ├── right_wheel.dae
│ │ ├── robot_chassis.dae
│ │ ├── rpi_4.dae
│ │ ├── rpi_camera.dae
│ │ ├── rpi_camera_mount.dae
│ │ ├── rpi_stand.dae
│ │ ├── rplidar_base.dae
│ │ ├── rplidar_stand.dae
│ │ └── rplidar_top.dae
│ ├── package.xml
│ ├── rviz/
│ │ ├── description.rviz
│ │ └── lidarbot_sim.rviz
│ └── urdf/
│ ├── camera.xacro
│ ├── gazebo_control.xacro
│ ├── imu.xacro
│ ├── inertial_macros.xacro
│ ├── lidar.xacro
│ ├── lidarbot.urdf.xacro
│ ├── lidarbot_core.xacro
│ └── ros2_control.xacro
├── lidarbot_gazebo/
│ ├── CMakeLists.txt
│ ├── config/
│ │ └── gazebo_params.yaml
│ ├── launch/
│ │ └── gazebo_launch.py
│ ├── package.xml
│ └── worlds/
│ └── obstacles.world
├── lidarbot_gz/
│ ├── CMakeLists.txt
│ ├── config/
│ │ └── lidarbot_bridge.yaml
│ ├── launch/
│ │ └── gz_launch.py
│ ├── models/
│ │ ├── construction_barrel/
│ │ │ ├── meshes/
│ │ │ │ └── construction_barrel.dae
│ │ │ ├── model-1_3.sdf
│ │ │ ├── model-1_4.sdf
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ └── construction_cone/
│ │ ├── meshes/
│ │ │ └── construction_cone.dae
│ │ ├── model-1_3.sdf
│ │ ├── model-1_4.sdf
│ │ ├── model.config
│ │ └── model.sdf
│ ├── package.xml
│ ├── urdf/
│ │ ├── lidarbot_gz.urdf.xacro
│ │ ├── lidarbot_gz_core.xacro
│ │ └── sensors.xacro
│ └── worlds/
│ └── obstacles.sdf
├── lidarbot_navigation/
│ ├── CMakeLists.txt
│ ├── config/
│ │ ├── ekf.yaml
│ │ └── nav2_params.yaml
│ ├── launch/
│ │ ├── localization_launch.py
│ │ └── navigation_launch.py
│ ├── lidarbot_navigation/
│ │ └── __init__.py
│ ├── maps/
│ │ ├── real_map3.pgm
│ │ ├── real_map3.yaml
│ │ ├── sim_map.data
│ │ ├── sim_map.pgm
│ │ ├── sim_map.posegraph
│ │ └── sim_map.yaml
│ ├── package.xml
│ ├── rviz/
│ │ └── lidarbot_nav.rviz
│ └── scripts/
│ └── trajectory_visualizer.py
├── lidarbot_slam/
│ ├── CMakeLists.txt
│ ├── config/
│ │ └── mapper_params_online_async.yaml
│ ├── launch/
│ │ └── online_async_launch.py
│ ├── package.xml
│ └── rviz/
│ └── lidarbot_slam.rviz
└── lidarbot_teleop/
├── CMakeLists.txt
├── config/
│ ├── joystick.yaml
│ └── twist_mux.yaml
├── launch/
│ └── joystick_launch.py
└── package.xml
================================================
FILE CONTENTS
================================================
================================================
FILE: .github/workflows/lidarbot_ci_action.yml
================================================
name: ROS2 CI
on:
pull_request:
push:
branches:
- main
jobs:
build:
runs-on: ubuntu-22.04
steps:
- name: Setup ROS2
uses: ros-tooling/setup-ros@v0.7
with:
required-ros-distributions: humble
- name: Install WiringPi i2c library for RPi 4
uses: actions/checkout@v4
- run: |
mkdir Downloads
git clone https://github.com/wbeebe/WiringPi.git
cd WiringPi/
./build
- name: Install dependencies for MPU6050 C++ library
run: sudo apt install libi2c-dev i2c-tools libi2c0
- name: Build Lidarbot packages
uses: ros-tooling/action-ros-ci@v0.4
with:
package-name: |
lidarbot
lidarbot_base
lidarbot_bringup
lidarbot_description
lidarbot_gazebo
lidarbot_gz
lidarbot_navigation
lidarbot_slam
lidarbot_teleop
target-ros2-distro: humble
skip-tests: true
- name: Upload Logs
uses: actions/upload-artifact@v4
with:
name: colcon-logs
path: ros_ws/log
if: always()
================================================
FILE: .gitignore
================================================
.vscode/
.vale.ini
styles/
================================================
FILE: LICENSE
================================================
BSD 3-Clause License
Copyright (c) 2023, Chinedu Amadi
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
================================================
FILE: README.md
================================================
# Lidarbot
<!--  -->
A differential drive robot is controlled using ROS2 Humble running on a Raspberry Pi 4 (running Ubuntu server 22.04). The vehicle is equipped with a Raspberry Pi camera for visual feedback and an RPlidar A1 sensor used for Simultaneous Localization and Mapping (SLAM), autonomous navigation using the Nav2 stack. Additionally, an MPU6050 inertial measurement unit (IMU) is employed by the `robot_localization` package on the robot, to fuse IMU sensor data and the wheel encoders data, using an Extended Kalman Filter (EKF) node, to provide more accurate robot odometry estimates.
Hardware components are written for the Waveshare Motor Driver HAT and MPU6050 sensor to be accessed by the `ros2_control` differential drive controller and IMU sensor broadcaster respectively, via the `ros2_control` resource manager.
<p align='center'>
<img src=docs/images/real_mapping.gif width="600">
</p>
A preprint of this work is available [here](http://dx.doi.org/10.13140/RG.2.2.15748.54408).
- [Lidarbot](#lidarbot)
- [🗃️ Package Overview](#️-package-overview)
- [🧰 Hardware](#hardware)
- [Part list](#part-list)
- [Project Wiring and Assembly](#project-wiring-and-assembly)
- [🔌 Installation](#-installation)
- [Development Machine setup](#development-machine-setup)
- [WiringPi](#wiringpi)
- [MPU6050 library](#mpu6050-library)
- [Sourcing ROS Installation](#sourcing-ros-installation)
- [Gazebo Fortress](#gazebo-fortress)
- [Display lidarbot model in RViz](#display-lidarbot-model-in-rviz)
- [Teleoperation](#teleoperation)
- [Twist mux](#twist-mux)
- [Lidarbot setup](#lidarbot-setup)
- [Motor Driver HAT](#motor-driver-hat)
- [Raspberry Pi Camera](#raspberry-pi-camera)
- [MPU6050 offsets](#mpu6050-offsets)
- [Network Configuration](#network-configuration)
- [ros2 control Framework](#ros2-control-framework)
- [Hardware components](#hardware-components)
- [System](#system)
- [Sensor](#sensor)
- [Actuator](#actuator)
- [Resource Manager](#resource-manager)
- [Controllers](#controllers)
- [Differential Drive Controller](#differential-drive-controller)
- [Joint State Broadcaster](#joint-state-broadcaster)
- [IMU Sensor Broadcaster](#imu-sensor-broadcaster)
- [Controller Manager](#controller-manager)
- [Test Drive](#test-drive)
- [Robot localization](#robot-localization)
- [Gazebo](#gazebo)
- [Physical](#physical)
- [Mapping](#mapping)
- [Gazebo](#gazebo-1)
- [Physical](#physical-1)
- [Navigation](#navigation)
- [Gazebo](#gazebo-2)
- [Physical](#physical-2)
- [ArUco package](#aruco-package)
- [Generate ArUco marker](#generate-aruco-marker)
- [Webcam calibration](#webcam-calibration)
- [Aruco trajectory visualizer node](#aruco-trajectory-visualizer-node)
- [Acknowledgment](#acknowledgment)
## 🗃️ Package Overview
- [`lidarbot_aruco`](./lidarbot_aruco/) : Contains configuration, launch and node files to use ArUco markers with lidarbot.
- [`lidarbot_base`](./lidarbot_base/) : Contains the ROS2 control hardware component for the lidarbot with low-level code for the Waveshare Motor Driver HAT.
- [`lidarbot_bringup`](./lidarbot_bringup/) : Contains hardware component for the MPU6050 module, launch files to bring up the camera, lidar and the real lidarbot.
- [`lidarbot_description`](./lidarbot_description/) : Contains the URDF description files for lidarbot, sensors and `ros2 control`.
- [`lidarbot_gazebo`](./lidarbot_gazebo/) : Contains configuration, launch and world files needed to simulate lidarbot in Gazebo Classic.
- [`lidarbot_gz`](./lidarbot_gz/) : Contains urdf, launch and world files needed to simulate lidarbot in Gazebo Fortress.
- [`lidarbot_navigation`](./lidarbot_navigation/) : Contains launch, configuration and map files used for lidarbot navigation.
- [`lidarbot_slam`](./lidarbot_slam/) : Contains configuration files for the slam toolbox and RViz, launch file to generate maps using SLAM.
- [`lidarbot_teleop`](./lidarbot_teleop/) : Contains configuration and launch files used to enable joystick control of the lidarbot in simulation and physically.
## 🧰 Hardware
### Part list
The following components were used in this project:
| | Part |
| --| --|
|1| Raspberry Pi 4 (4 GB)|
|2| SanDisk 32 GB SD Card (minimum)|
|3| [Two wheel drive robot chassis kit](https://www.amazon.com/perseids-Chassis-Encoder-Wheels-Battery/dp/B07DNYQ3PX/ref=sr_1_9?crid=3T8FVRRMPFCIX&keywords=two+wheeled+drive+robot+chassis&qid=1674141374&sprefix=two+wheeled+drive+robot+chas%2Caps%2C397&sr=8-9)|
|4| [Waveshare Motor Driver HAT](https://www.waveshare.com/wiki/Motor_Driver_HAT)|
|5| 2 x [Motors with encoders and wire harness](https://s.click.aliexpress.com/e/_DBL19Mr)|
|6| MPU6050 board|
|7| [RPlidar A1](https://s.click.aliexpress.com/e/_DdPdRS7)|
|8| Raspberry Pi camera v1.3|
|9| [3D printed stands for RPlidar A1 and RPi 4](https://www.thingiverse.com/thing:3970110)|
|10| Mount for Raspberry Pi camera|
|11| Powerbank for RPi 4 (minimum output: 5V 3A)|
|12| Gamepad|
|13| [Mini Travel Router (optional)](https://s.click.aliexpress.com/e/_DcgfT61)|
|14| 3 Slot 18650 battery holder|
|15| 3 x 18650 batteries to power Motor Driver HAT|
|16| Female to Female Dupont jumper cables|
|17| Spare wires|
|18| Logitech C270 webcam (optional)|
Some other tools or parts used in the project are as follows:
| | Tool/Part |
| --| --|
|1| Soldering iron|
|2| 3D printer|
|3| Screwdriver set|
|4| Double-sided tape|
Additionally, some nylon stand-offs were used in between the Raspberry Pi 4 and its 3D printed stand to make it easier to plug in the USB power cable from the powerbank.
### Project Wiring and Assembly
The electronic components of the lidarbot are connected as shown below.
<p align="center">
<img title='Wiring diagram' src=docs/images/lidarbot_wiring.png width="800">
</p>
The MPU6050 board pins were connected to the Raspberry Pi 4 GPIO pins as follows for use with the I2C communication protocol:
| MPU6050 board | GPIO.BOARD| GPIO.BCM|
| ----------- | ------------| ------ |
| VCC | 3.3V | 3.3V |
| GND | GND | GND |
| SCL | 05 | GPIO03 |
| SDA | 03 | GPIO02 |
In a previous hardware version of lidarbot, photo interrupters were used with encoder disks with 20 slots (included in the robot chassis kit). However, this setup proved restrictive in yielding satisfactory results in navigation due to the low number of encoder ticks, which was only **20** for a **360 degree** turn of the motor. Therefore, the robot chassis kit motors were replaced with the ones below with built-in encoders --- which have encoder ticks of approximately **1084**, calculated using this [guide](https://automaticaddison.com/calculate-pulses-per-revolution-for-a-dc-motor-with-encoder/) from Automatic Addison.
These are the new motors used for lidarbot:
<p align="center">
<img title='Motors' src=docs/images/motors.jpg width="400">
<img title='Motor pins' src=docs/images/motor_pins.jpg width="400">
</p>
The pins on right and left motors are connected to the GPIO pins as follows:
| Right motor pins | GPIO.BOARD | GPIO.BCM|
| ----------- | ------------| ------ |
| C1 (or Encoder A) | 22 | GPIO25 |
| C2 (or Encoder B) | 16 | GPIO23 |
| VCC | 5V | 5V |
| GND | GND | GND |
| Left motor pins| GPIO.BOARD | GPIO.BCM|
| ----------- | ------------| ------ |
| C1 (or Encoder A) | 18 | GPIO24 |
| C2 (or Encoder B) | 15 | GPIO22 |
| VCC | 5V | 5V |
| GND | GND | GND |
Where,
C1 - counts the respective motor pulses and
C2 - checks the direction of motion of the respective motor, that is, forward or reverse.
<p align="center">
<img title='MPU6050' src=docs/images/mpu6050.jpg width="400">
<img title='Encoders' src=docs/images/encoders.jpg width="400">
</p>
The screw terminal blocks on the Motor Driver HAT ([shown below](https://www.waveshare.com/wiki/Motor_Driver_HAT)) are connected to the motor pins, M+ and M-, and battery holder cables as follows:
| Motor Driver HAT pin | Connected to|
| -- | -- |
| MA1 | Red wire (Left motor)|
| MA2 | Black wire (Left motor)|
| GND | Black wire (battery holder) |
| VIN | Red wire (battery holder) |
| MB1 | Red wire(Right motor)|
| MB2 | Black wire (Right motor)|
<p align="center">
<img title='Motor Driver HAT' src=docs/images/Motor_Driver_HAT.png width="400">
</p>
Solder the cables (provided) to the motors. Spare wires might be needed if the provided ones are too short to reach the Motor Driver Hat. Should the wheel(s) move in the direction opposite of what is expected, exchange the respective motor cables screwed into the terminal blocks.
Finally, the Raspberry Pi camera is connected to the ribbon slot on the Raspberry Pi 4 and the RPlidar A1 sensor is plugged into one of the RPi 4's USB ports.
<p align='center'>
<img title='Top View' src=docs/images/top_view.jpg width="600">
</p>
<p align="center">
<img title='Side View' src=docs/images/side_view.jpg width="600">
</p>
## 🔌 Installation
### Development Machine setup
A development machine or PC (laptop or desktop) is used to run more computationally intensive applications like Gazebo and Rviz. Additionally, the PC can be used to remotely control lidarbot.
Ubuntu 22.04 LTS is required for this project to work with ROS2 Humble. Ubuntu 22.04 LTS can be installed on a PC by following [this guide](https://ubuntu.com/tutorials/install-ubuntu-desktop#1-overview). The ROS2 Humble installation procedure is available [here](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html). The Desktop version is installed on the PC (which includes RViz):
```
sudo apt install ros-humble-desktop
```
Then install the ROS development tools:
```
sudo apt install ros-dev-tools
```
After the ROS2 Humble installation, create a workspace on the PC/development machine and clone this repository:
```
mkdir -p ~/dev_ws/src
cd ~/dev_ws/src
git clone https://github.com/TheNoobInventor/lidarbot.git .
```
Next install all the [ROS dependencies](https://docs.ros.org/en/humble/Tutorials/Intermediate/Rosdep.html) for the lidarbot packages:
```
cd ~/dev_ws
sudo rosdep init
rosdep update
rosdep install --from-paths src --ignore-src --rosdistro humble -r -y
```
Any ROS packages referred to subsequently are assumed to be installed using the `rosdep install` command above unless it is explicitly specified.
Two more dependencies need to be met before building the workspace: installing the WiringPi i2c library to use the Raspberry Pi 4 GPIO pins and dependencies for the MPU6050 RPi 4 C++ library.
#### WiringPi
To be able to utilize the GPIO pins of the Raspberry Pi 4 and program them using C/C++, an unofficial WiringPi was installed. This is required as hardware interfaces used by `ros2_control` are currently written only in C++ and low-level communication between Waveshare's Motor Driver HAT and `ros2_control` is needed.
The library is installed by executing the following commands in a terminal:
```
cd ~/Downloads
git clone https://github.com/wbeebe/WiringPi.git
cd WiringPi/
./build
```
To check the current gpio version run this:
```
gpio -v
```
The reference article for the WiringPi library can be found [here](https://arcanesciencelab.wordpress.com/2020/10/29/getting-wiringpi-to-work-under-ubuntu-20-10-on-a-raspberry-pi-4b-w-4gb/).
#### MPU6050 library
Alex Mous' [C/C++ MPU6050 library](https://github.com/alex-mous/MPU6050-C-CPP-Library-for-Raspberry-Pi
) for Raspberry Pi 4, with modifications to incorporate quaternions, was used to setup the `ros2_control` IMU sensor broadcaster in the [`lidarbot_bringup`](./lidarbot_bringup/) package.
Recall that the MPU6050 module uses the I2C communication protocol, the I2C dependencies for using this library are installed with:
```
sudo apt install libi2c-dev i2c-tools libi2c0
```
#### Sourcing ROS Installation
To avoid manually sourcing the ROS installation (or underlay) in each terminal window opened, and if ROS2 Humble is the only distribution on the PC, the command to source the underlay is added to the respective shell configuration file.
Using bash:
```
echo "source /opt/ros/humble/setup.bash" >> $HOME/.bashrc
```
Using zsh:
```
echo "source /opt/ros/humble/setup.zsh" >> $HOME/.zshrc
```
Additionally, to avoid manually sourcing our workspace (or overlay), add the command to source the workspace to the respective configuration file.
Using bash:
```
echo "source ~/dev_ws/install/setup.bash" >> $HOME/.bashrc
source $HOME/.bashrc
```
Using zsh:
```
echo "source ~/dev_ws/install/setup.zsh" >> $HOME/.zshrc
source $HOME/.zshrc
```
The command: `source $HOME/.zshrc` sources the configuration file for use in the current terminal. However, this step is not necessary for terminal windows opened hereafter.
---
Finally, navigate to the workspace directory and run the build command:
```
cd ~/dev_ws
colcon build --symlink-install
```
The `--symlink-install` argument uses symlinks instead of copies which saves you from having to rebuild every time you [tweak certain files](https://articulatedrobotics.xyz/ready-for-ros-5-packages/).
#### Gazebo Fortress
*UPDATE: Previously, [Gazebo Classic](https://classic.gazebosim.org/) was used as the simulator for the project but Gazebo Classic has been [deprecated](https://gazebosim.org/docs/latest/gazebo_classic_migration/). The Gazebo Classic lidarbot package is still available here, [`lidarbot_gazebo`](./lidarbot_gazebo/), however, you might encounter installation issues with Classic. It is recommended to [migrate to Gazebo](https://gazebosim.org/docs/latest/gazebo_classic_migration/ ), formerly called "Ignition", and choosing the Fortress release for use with Ubuntu 22.04.*
Gazebo Fortress is used as the simulator for lidarbot. It can be installed with ROS following the steps outlined in this [guide](https://gazebosim.org/docs/fortress/ros_installation/).
#### Display lidarbot model in RViz
The installed [xacro](https://index.ros.org/p/xacro/github-ros-xacro/#humble) tool dependency is used to process the lidarbot URDF files and combine them into a single complete URDF file.
The `description_launch.py` launch file displays the model in RViz:
```
ros2 launch lidarbot_description description_launch.py
```
<p align='center'>
<img src=docs/images/lidarbot_rviz.png width="800">
</p>
The [joint_state_publisher_gui](https://index.ros.org/p/joint_state_publisher_gui/github-ros-joint_state_publisher/#humble) package is used to bringup a window with sliders to move non-static links in RViz. Set the `use_gui` argument to `true` to turn the left and right wheels of lidarbot:
```
ros2 launch lidarbot_description description_launch.py use_gui:=true
```
<p align='center'>
<img src=docs/images/joint_state_publisher_gui.gif width="800">
</p>
The different arguments for the launch file, and their default values, can be viewed by adding `--show-args` at the end of launch command:
```
ros2 launch lidarbot_description description_launch.py --show-args
```
#### Teleoperation
A wireless gamepad, like the one shown below, is used to control lidarbot both in simulation and physically.
<p align='center'>
<img src=docs/images/wireless_gamepad.jpg width="600">
</p>
The [joy_tester](https://index.ros.org/p/joy_tester/#humble) package is used to test and map the gamepad (joystick) keys to control lidarbot. To use it, plug in the USB dongle in the PC, then run:
```
ros2 run joy joy_node
```
And the following, in a new terminal:
```
ros2 run joy_tester test_joy
```
This opens a GUI window like the one shown below,
<p align='center'>
<img src=docs/images/joy_tester.png width="600">
</p>
Click each button and move each stick of the gamepad to confirm that the actions are shown in GUI. The numbers correspond to the axis of the buttons and joystics (sticks) that will be used in mapping the movements of lidarbot.
The gamepad configuration for this project is in [`joystick.yaml`](./lidarbot_teleop/config/joystick.yaml), where:
| Button/stick | Button/stick axis | Function |
| :--: | :--: | -- |
| L1 button | 4 | Hold this enable button to move robot at normal speed|
| Left stick | 2 | Move stick forward or backward for linear motion of the robot |
| Right stick | 1 | Move stick left or right for angular motion of the robot|
Setting `require_enable_button` to `true` ensures that L1 has to be held before using the sticks to move the robot and stops the robot once L1 is no longer pressed.
To enable turbo mode for faster speed, the `enable_turbo_button` option in the config file can be set to an unused button axis.
The `joy_node` parameter, `deadzone`, specifies the amount a joystick has to be moved for it to be [considered to be away from the center](https://github.com/ros-drivers/joystick_drivers/blob/ros2/joy/README.md). This parameter is normalized between `-1` and `1`. A value of `0.25` indicates that the joytsick has to be moved `25%` of the way to the edge of an axis's range before that axis will output a non-zero value.
The `deadzone` parameter should be tuned to suit the performance of user's game controller.
#### Twist mux
The [`twist_mux`](https://index.ros.org/p/twist_mux/github-ros-teleop-twist_mux/#humble) package is used to multiplex several velocity command sources, used to move the robot with an unstamped [geometry_msgs::Twist](http://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html) message, into a single one. These sources are assigned priority values to allow a velocity source to be used or disabled. In this project, the command velocity sources are from the joystick and navigation.
The `twist_mux` configuration file is in [`twist_mux.yaml`](./lidarbot_teleop/config/twist_mux.yaml), and is used in the Gazebo and lidarbot bringup launch files, [`gz_launch.py`](./lidarbot_gz/launch/gz_launch.py) and [`lidarbot_bringup_launch.py`](./lidarbot_bringup/launch/lidarbot_bringup_launch.py) respectively.
It can be observed from the configuration file, that the joystick commmand velocity source has a higher priority, with an assigned value of `100`, compared to the navigation velocity source that is assigned a value of `10`.
```
twist_mux:
ros__parameters:
topics:
navigation:
topic : cmd_vel
timeout : 0.5
priority: 10
joystick:
topic : cmd_vel_joy
timeout : 0.5
priority: 100
```
### Lidarbot setup
To install ROS2 Humble on the Raspberry Pi, Ubuntu Server 22.04 was first flashed on a 32GB micro SD card, this process is detailed in this [guide](https://ubuntu.com/tutorials/how-to-install-ubuntu-on-your-raspberry-pi#1-overview).
After inserting the SD card and booting up the Pi, the environment for ROS2 Humble is setup by following this [guide](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html). Afterwards, ROS-Base (Bare Bones) and ROS development tools are installed:
```
sudo apt install ros-humble-ros-base ros-dev-tools
```
Similarly, after the ROS2 Humble installation, create a workspace on the Raspberry Pi and clone this repository:
```
mkdir -p ~/robot_ws/src
cd ~/robot_ws/src
git clone https://github.com/TheNoobInventor/lidarbot.git .
```
Install ROS dependencies:
```
cd ~/robot_ws
sudo rosdep init
rosdep update
rosdep install --from-paths src --ignore-src -r -y \
--skip-keys "rviz2 gazebo_ros2_control gazebo_ros_pkgs" --rosdistro humble
```
`rviz2` and the Gazebo related packages are skipped in the ROS dependency installation process as they are only run on the PC and not on the robot --- the `rosdep` keys for the ROS2 Humble distribution are available [here](https://github.com/ros/rosdistro/blob/master/humble/distribution.yaml).
[WiringPi i2c library](#wiringpi) and [MPU6050 RPi 4 C++ library](#mpu6050-library) are also installed before building the workspace --- a `Downloads` directory will need to be created to clone the WiringPi files.
Likewise to avoid manually sourcing the underlay and overlay, the same steps employed in the [development machine setup](#sourcing-ros-installation) are followed but replacing `dev_ws` with `robot_ws` where necessary.
Afterwards, navigate to the workspace directory then run the build command:
```
cd ~/robot_ws
colcon build --symlink-install
```
#### Motor Driver HAT
Waveshare's Motor Driver HAT was used to control the motors of lidarbot. The relevant files are found in the [`include`](./lidarbot_base/include/lidarbot_base/) and [`src`](./lidarbot_base/src/) directories of the [`lidarbot_base`](./lidarbot_base/) package, the file tree of these directories are as shown,
```
├── CMakeLists.txt
├── include
│ └── lidarbot_base
│ ├── Debug.h
│ ├── DEV_Config.h
│ ├── lidarbot_hardware.hpp
│ ├── MotorDriver.h
│ ├── motor_encoder.h
│ ├── PCA9685.h
│ └── wheel.hpp
├── lidarbot_hardware.xml
├── package.xml
└── src
├── DEV_Config.c
├── lidarbot_hardware.cpp
├── motor_checks_client.cpp
├── motor_checks_server.cpp
├── MotorDriver.c
├── motor_encoder.c
├── PCA9685.c
└── wheel.cpp
```
The following table summarizes the package files specifying those that were made [available by Waveshare](https://www.waveshare.com/wiki/Motor_Driver_HAT), the newly added ones and the functions they serve.
| File(s) | New | Modified | Function |
| ----------- | ------------| ------ | ------ |
| `Debug.h` | No | No | Output debug information with `printf`. |
| `DEV_Config.h`, `DEV_Config.c` | No | Yes | Configure underlying device interfaces and protocols. |
| `lidarbot_hardware.hpp`, `lidarbot_hardware.cpp` | Yes | No | Configure the WaveShare Motor Driver HAT system hardware component. It reads the motor encoder values to calculate the wheel positions and velocities. It also sends commands to the drive the motors of each wheel proportional to the velocity commands received from the `ros2_control` differential driver controller. |
| `motor_checks_client.cpp` | Yes | No | Request motor tests to confirm motor connections and the motor operation. |
| `motor_checks_server.cpp` | Yes | No | Run requested motor tests. |
| `MotorDriver.h`, `MotorDriver.c` | No | Yes | Program the TB6612FNG motor Integrated Circuit (IC) to drive the motors. |
| `motor_encoder.h`, `motor_encoder.c` | Yes | No | Calculate encoder ticks and set the speed each motor should run at according to the commands received from the hardware component. |
| `PCA9685.h`, `PCA9685.c` | No | No | Program the PCA9685 chip to adjust the motor speed using Pulse Width Modulation (PWM). |
| `wheel.hpp`, `wheel.cpp` | Yes | No | Configure motor wheel parameters. |
The hierarchy of motor control is shown in the image below.
<p align='center'>
<img src=docs/images/motor_control_hierarchy.jpg width="600">
</p>
The main reasons for modifying the Waveshare files was to enable the use of the WiringPi library and also to find a workaround to determining the motor direction of rotation --- this was made relatively easier since the motor encoders used here have hall effect sensors affixed to them as shown below.
<p align='center'>
<img src=docs/images/hall_effect_sensors.png width="800">
</p>
Hall effect sensors in motor encoders detect changes in the magnetic field associated with the motor's rotation. These changes can be used by the motor encoder to determine the position of the motor. [As the motor rotates](https://automaticaddison.com/calculate-pulses-per-revolution-for-a-dc-motor-with-encoder/), it generates alternating electrical signals of high and low voltage, and each time the signal goes from from low high (that is, it is rising), that counts as a single ***pulse***. The image below, from this [guide](https://automaticaddison.com/calculate-pulses-per-revolution-for-a-dc-motor-with-encoder/) from Automatic Addison, visualizes this.
<p align='center'>
<img src=docs/images/time_interval.jpg width="600">
</p>
The encoder pulse, sometimes referred to as a ***tick***, for a 360 degree turn of the motor is used as the wheel odometry data of the robot. This data will be used with the MPU6050 IMU sensor data using an Extended Kalman Filter with the [robot_localization](#robot-localization) package to provide more accurate robot odometry estimates. Recall that the encoder ticks for the motors used here is **1084**, for a 360 degree turn of the motor.
The library dependencies for using the Motor Driver HAT were met after installing the MPU6050 library.
#### Raspberry Pi Camera
The following packages are installed to use the Raspberry Pi Camera v1.3:
```
sudo apt install libraspberrypi-bin v4l-utils raspi-config
```
Support for the RPi camera v1.3 will need to be enabled by navigating the menu options after running the following command:
```
sudo raspi-config
```
<p align='center'>
<img src=docs/images/raspi_config_1.png width="400">
<img src=docs/images/raspi_config_2.png width="400">
</p>
<p align='center'>
<img src=docs/images/raspi_config_3.png width="400">
<img src=docs/images/raspi_config_4.png width="400">
</p>
Confirm the RPi camera is connected by running this command:
```
vcgencmd get_camera
```
This should output the following result:
```
supported=1 detected=1, libcamera interfaces=0
```
#### MPU6050 offsets
Prior to using the [IMU sensor broadcaster](#imu-sensor-broadcaster), the MPU6050 module needs to be calibrated to filter out its sensor noise/offsets. This is done in the following steps:
- Place lidarbot on a flat and level surface and unplug the RPlidar.
- Generate the MPU6050 offsets. A Cpp executable is created in the CMakeLists.txt file of the `lidarbot_bringup` package before generating the MPU6050 offsets. This section of the [CMakeLists.txt](./lidarbot_bringup/CMakeLists.txt) file is shown below:
```
# Create Cpp executable
add_executable(mpu6050_offsets src/mpu6050_lib.cpp src/mpu6050_offsets.cpp)
# Install Cpp executables
install(TARGETS
mpu6050_offsets
DESTINATION lib/${PROJECT_NAME}
)
```
Build the `lidarbot_bringup` package:
```
colcon build --symlink-install --packages-select lidarbot_bringup
```
Run the executable:
```
ros2 run lidarbot_bringup mpu6050_offsets
```
Which outputs something like this:
```
Please keep the MPU6050 module level and still. This could take a few minutes.
Calculating offsets ...
Gyroscope offsets:
------------------
X: -104.689
Y: 651.005
Z: -158.596
Accelerometer offsets:
----------------------
X: -13408.8
Y: 2742.39
Z: -14648.9
Include the obtained offsets in the respective macros of the mpu6050_lib.h file.
```
- Calibrate the MPU6050 module. Substitute the generated offsets into this section of the [`mpu6050_lib.h`](./lidarbot_bringup/include/lidarbot_bringup/mpu6050_lib.h) file:
```
//Offsets - supply your own here (calculate offsets with getOffsets function)
// Gyroscope
#define G_OFF_X -105
#define G_OFF_Y 651
#define G_OFF_Z -159
// Accelerometer
#define A_OFF_X -13409
#define A_OFF_Y 2742
#define A_OFF_Z -14649
```
- Afterwards, the `lidarbot_bringup` package is rebuilt to reflect any changes made to the `mpu6050_lib.h` file.
The MPU6050 module is set to its most sensitive gyroscope and accelerometer ranges, which can be confirmed (or changed) at the top of the `mpu6050_lib.h` file.
## Network Configuration
Both the development machine and lidarbot need to be connected to the same local network as a precursor to bidirectional communication between the two systems. This [guide](https://roboticsbackend.com/ros2-multiple-machines-including-raspberry-pi/) by Robotics Backend was used in configuring the network communication.
To ensure communication between the dev machine and lidarbot, firstly, the firewall on the development machine had to be disabled (the firewall on the Ubuntu server was disabled by default):
```
sudo ufw disable
```
The next step is to export `ROS_DOMAIN_ID` number, a number between 0 (by default) and 101, to the shell configurations of both systems:
```
echo "export ROS_DOMAIN_ID=31" >> ~/.zshrc
```
This step is necessary particularly if there are other robots and development machines on the same local network. This [YouTube video](https://www.youtube.com/watch?v=qSjNQC2AT_4) and [article](https://docs.ros.org/en/humble/Concepts/Intermediate/About-Domain-ID.html) go into more detail about `ROS_DOMAIN_ID`.
Then source the shell configuration file:
```
source $HOME/.zshrc
```
Both systems might need to be rebooted to effect these changes.
A static IP address was assigned to lidarbot on the router for easy discoverability on the network. Furthermore, it is advisable to use a router that supports at least the WiFi 5 wireless standard to avoid excessive data lag on RViz and terminal crashes when recording a [ros2bag](https://index.ros.org/r/rosbag2/github-ros2-rosbag2/#humble) for instance; in relation to ros2bags, changing the default data storage format to [MCAP](https://foxglove.dev/blog/announcing-the-mcap-storage-plugin-for-ros2) should [improve read and write performance](https://adrian-website.mcap.pages.dev/guides/benchmarks/rosbag2-storage-plugins).
A dedicated network travel router, following this setup by [Articulated Robotics](https://articulatedrobotics.xyz/tutorials/ready-for-ros/networking#dedicated-network) was used in this project to establish a reliable connection between the development machine and lidarbot.
Additional network troubleshooting information can be found [here](https://docs.ros.org/en/humble/How-To-Guides/Installation-Troubleshooting.html).
## ros2 control Framework
To control the motion of lidarbot both in simulation and physically, the `ros2_control` framework was used. It is defined as a [hardware-agnostic control framework](https://control.ros.org/master/doc/resources/resources.html#roscon-fr-2022) focusing on the modularity of control systems for robots, sharing of controllers and real-time performance. It provides hardware abstraction and low-level management utility, for instance, hardware lifecycle, communication and access control, which is useful for other frameworks like the manipulation path planning and autonomous navigation frameworks, [MoveIt2](https://moveit.picknik.ai/main/index.html) and [Nav2](https://ieeexplore.ieee.org/iel7/9340668/9340635/09341207.pdf) respectively.
The `ros2_control` framework can be thought of as a [middleman](https://automaticaddison.com/how-to-control-a-robotic-arm-using-ros-2-control-and-gazebo/) between the robot hardware and the robot software and is comprised of the following parts:
1. Hardware Components
2. Resource Manager
3. Controllers
4. Controller Manager
### Hardware components
Hardware components are drivers for physical hardware and represent its abstraction in the `ros2_control` framework. These components are usually provided by robot or hardware manufacturers, otherwise the components would either have to be sourced from the ROS community or written from scratch using this [guide](https://control.ros.org/master/doc/ros2_control/hardware_interface/doc/writing_new_hardware_component.html) made available by the `ros2_control` development team. Hardware components are written in `C++` and exported as plugins using the [`pluginlib`](https://index.ros.org/p/pluginlib/#humble)-library; these plugins are dynamically loaded at runtime by the [Resource Manager](#resource-manager).
Hardware components are configured in the robot's Unified Robot Description Format (URDF) file --- an XML file that describes the model of the robot --- using the `<ros2_control>` [tag](https://control.ros.org/master/doc/getting_started/getting_started.html#hardware-description-in-urdf) and the recommended `xacro` (XML) macro files.
There are 3 basic types of components:
#### System
This refers to a hardware setup made up of more than 1 Degree of Freedom (DOF) which contains multiple joints and sensors. This component type offers a lot of flexibility in handling hardware setups which can range from mobile robots to humanoid robot hands and more. The Waveshare Motor Driver HAT controls the two motors (or two wheel joints) on lidarbot, hence the robot base is classifed as a system component.
A system component has reading and writing capabilities, these are expressed as state interfaces and command interfaces respectively. The system component configuration for the robot base with a velocity command interface and state interfaces for both the velocity and position is shown in the code snippet below, available in this [xacro file](./lidarbot_description/urdf/ros2_control.xacro).
```
<ros2_control name='RealRobot' type='system'>
<hardware>
<plugin>lidarbot_base/LidarbotHardware</plugin>
<param name='left_wheel_name'>left_wheel_joint</param>
<param name='right_wheel_name'>right_wheel_joint</param>
<param name='enc_ticks_per_rev'>1084</param>
<param name='loop_rate'>30.0</param>
</hardware>
<joint name='left_wheel_joint'>
<command_interface name='velocity'>
<param name='min'>-10</param>
<param name='max'>10</param>
</command_interface>
<state_interface name='velocity'/>
<state_interface name='position'/>
</joint>
<joint name='right_wheel_joint'>
<command_interface name='velocity'>
<param name='min'>-10</param>
<param name='max'>10</param>
</command_interface>
<state_interface name='velocity'/>
<state_interface name='position'/>
</joint>
</ros2_control>
```
A similar system hardware component is configured for Gazebo Fortress in this [URDF](./lidarbot_gz/urdf/lidarbot_gz.urdf.xacro) file.
#### Sensor
A sensor provides information about changes to a physical phenomenon it is observing. A sensor component has only reading capabilities and is related to a joint or a link in the URDF file. This component type can be configured as an externally connected sensor, with its own `<ros2_control>` tags, or as an integrated sensor embedded in system component `<ros2_control>` tags. The MPU6050 module is configured as an externally connected sensor component and is associated with a fixed link named `imu_link`. This configuration is shown in the code snippet below which is available in the MPU6050 section of this [xacro file](./lidarbot_description/urdf/ros2_control.xacro).
```
<ros2_control name='MPU6050' type='sensor'>
<hardware>
<plugin>lidarbot_bringup/MPU6050Hardware</plugin>
<param name='sensor_name'>mpu6050</param>
<param name='frame_id'>imu_link</param>
</hardware>
<sensor name='mpu6050'>
<state_interface name='orientation.x'/>
<state_interface name='orientation.y'/>
<state_interface name='orientation.z'/>
<state_interface name='orientation.w'/>
<state_interface name='angular_velocity.x'/>
<state_interface name='angular_velocity.y'/>
<state_interface name='angular_velocity.z'/>
<state_interface name='linear_acceleration.x'/>
<state_interface name='linear_acceleration.y'/>
<state_interface name='linear_acceleration.z'/>
</sensor>
</ros2_control>
```
However, a standalone [IMU sensor plugin](https://gazebosim.org/docs/fortress/sensors/#imu-sensor) was used to simulate an [IMU sensor](./lidarbot_gz/urdf/sensors.xacro) in Gazebo Fortress.
#### Actuator
This component type only has 1 DOF. Some examples of these include motors and valves. Actuator components are related to a single joint and have both reading and writing capabilities, however, obtaining state feedback is not mandatory if is inaccessible; for instance, controlling a DC motor that has no encoders.
The image below (adapted from [Denis Štogl](https://vimeo.com/649651707) (CC-BY)) shows the system and sensor components used for the lidarbot base and the MPU6050 module respectively — custom hardware components were written for the [Waveshare Motor Driver HAT](./lidarbot_base/src/lidarbot_hardware.cpp) and the [MPU6050 module](./lidarbot_bringup/src/mpu6050_hardware_interface.cpp) using the [MPU6050 library](#mpu6050-library).
<p align="center">
<img title='hardware components' src=docs/images/hardwarecomp.png width="400">
</p>
The coloured icons represent the state and command interfaces of the hardware components. The state interfaces for the respective cartesian axes, associated with the MPU6050 module, were merged into one for clarity.
### Resource Manager
The Resource Manager (RM) abstracts the hardware components for the `ros2_control` framework. The RM is responsible for loading the components using the `pluginlib`-library, managing their lifecycle and components’ state and command interfaces. In the control loop execution, which is managed by the Controller Manager, the RM’s `read()` and `write()` methods handle the communication with the hardware components.
The abstraction provided by RM allows reuse of implemented hardware components. A sensor component, like the MPU6050 module, has only reading capabilities, thus it can be shared by multiple controllers simultaneously. However, components with command interfaces require exclusive access by only one controller.
The bidirectional arrows in the figure below (adapted from [Denis Štogl](https://vimeo.com/649651707) (CC-BY)) indicate interfaces that have both read and write capabilities, but interfaces that only provide state feedback are represented with unidirectional arrows.
<p align="center">
<img title='resource manager' src=docs/images/resman.png width="400">
</p>
### Controllers
The controllers in the `ros2_control` framework are based on control theory which compare the reference value with the measured output and, based on this error, calculate a system’s input. Controllers access the latest hardware lifecycle state when the Controller Manager’s `update()` method is called and enables them to write to the hardware command interfaces.
There are a number of controllers available for specific hardware configurations, such as ***Ackermann steering controller***, ***bicycle steering controller***, ***gripper controller*** and ***differential drive controller*** (which is used on lidarbot). The full list of controllers is available on the `ros2_controllers` [page](https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html). Similar to hardware components, a [guide](https://control.ros.org/master/doc/ros2_controllers/doc/writing_new_controller.html) is also provided on how to write a custom controller if a particular hardware configuration is missing.
#### Differential Drive Controller
The differential drive controller receives `Twist.msg` velocity command messages from the Nav2 stack which are written to the velocity command interfaces of the robot base system hardware component. This in turn drives the motors in accordance with the received velocity command interfaces. This sequence of commands is illustrated in the next image.
#### Joint State Broadcaster
Broadcasters only have reading abilities and are used to publish sensor data from hardware components to ROS topics. The joint state broadcaster reads all state interfaces and reports them on the `/joint_states` and `/dynamic_joint_state` topics. These topics are used by RViz to display the current state of the robot joints.
#### IMU Sensor Broadcaster
The IMU sensor broadcaster on lidarbot publishes `Imu.msg` messages on the topic, `imu_broadcaster/imu`, from the MPU6050 IMU hardware sensor component. The IMU sensor data is utilized by the [`robot_localization`](#robot-localization) package to fuse wheel odometry data with an Extended Kalman Filter (EKF). This is used to improve the robot’s localization for autonomous navigation with the Nav2 framework. The topic connections between Nav2, relevant packages and the lidarbot controllers are presented in the [navigation](#navigation) subsection.
The lidarbot differential drive controller and IMU sensor broadcaster are configured in the [`controllers.yaml`](./lidarbot_bringup/config/controllers.yaml) file in the bringup package; the joint state broadcaster here uses the default parameters so no configuration is needed.
In configuring the IMU sensor broadcaster, the static orientation, velocity and linear acceleration covariances, which have a row major arrangement about the x, y and z axes, need to be calculated. A [covariance matrix](https://www.cuemath.com/algebra/covariance-matrix/), sometimes referred to as a variance-covariance matrix, is a square matrix where the diagonal elements represent the variance and the off-diagonal elements represent the covariance.
Visualizing the static velocity covariance, for instance, with the matrix below, the diagonal elements represent the variance of a chosen sample data size of velocity measurements for the respective axes. While the off-diagonal elements represent the covariance between velocities of two axes over a chosen sample data size.
┌──────────┬──────────┬──────────┐
│ Var(x,x) │ Cov(x,y) │ Cov(x,z) │
├──────────┼──────────┼──────────┤
│ Cov(y,x) │ Var(y,y) │ Cov(y,z) │
├──────────┼──────────┼──────────┤
│ Cov(z,x) │ Cov(z,y) │ Var(z,z) │
└──────────┴──────────┴──────────┘
The matrix above is represented as a 9 element array, in the IMU Sensor Broadcaster configuration of the [`controllers.yaml`](./lidarbot_bringup/config/controllers.yaml) file for the respective covariances to be calculated.
However, only the variance array elements are calculated. It is assumed that the different axes do not vary together, therefore the off-diagonal elements/covariances are zero. The [`mpu6050_covariances.cpp`](./lidarbot_bringup/src/mpu6050_covariances.cpp) file can be modified to incorporate the covariance calculations later on.
The variance was calculated with a sample data size of **500** points, with a delay of **0.25s** to read non-consecutive values from the MPU6050 module.
Keep the module still then run this node to generate the variances:
```
ros2 run lidarbot_bringup mpu6050_covariances
```
An output similar to the following will be shown in the terminal window
```
Please keep the MPU6050 module level and still.
Reading and appending 500 sensor data points to respective arrays, it may take a while ...
Calculating variances ...
static_covariance_orientation: [5.46409e-06, 0.0, 0.0, 5.72254e-06, 0.0, 0.0, 4.22433e-07, 0.0, 0.0]
static_covariance_angular_velocity: [2.01015e-06, 0.0, 0.0, 1.9657e-06, 0.0, 0.0, 8.13776e-07, 0.0, 0.0]
static_covariance_linear_acceleration: [0.000632951, 0.0, 0.0, 0.000801987, 0.0, 0.0, 0.00117363, 0.0, 0.0]
Paste covariance arrays in the imu_broadcaster ros__parameters section in lidarbot_bringup/config/controllers.yaml.
```
These arrays are then pasted in the `imu_broadcaster ros__parameters` section of the [`controllers.yaml`](./lidarbot_bringup/config/controllers.yaml) file to complete the IMU Sensor Broadcaster configuration.
### Controller Manager
The Controler Manager (CM) is responsible for managing the lifecycle of controllers (for instance, loading, activating, deactivating and unloading respective controllers) and the interfaces they require. Through the RM, the CM has access to the hardware component interfaces. The Controller Manager connects the controllers and hardware-abstraction sides of the `ros2_control` framework, and also serve as the entry-point for users via ROS services using the [`ros2 cli`](https://control.ros.org/master/doc/ros2_control/ros2controlcli/doc/userdoc.html).
The Controller Manager matches required and provided interfaces, grants contollers access to hardware when enabled, or reports an error if there is an access conflict. The execution of the control-loop is managed by the CM's `update()` method which reads data from the hardward components, updates the outputs of all active controllers, and writes the result to the components.
In essence, the CM manages the controllers, which calculate the commands needed to make the [robot move as desired](https://automaticaddison.com/how-to-control-a-robotic-arm-using-ros-2-control-and-gazebo/).
The following image (adapted from [Denis Štogl](https://vimeo.com/649651707) (CC-BY)) illustrates all the parts of the `ros2_control` framework modified for use on lidarbot.
<p align="center">
<img title='ros2 control architecture' src=docs/images/ros2_control_arch.png width="800">
</p>
The `ros2_control` framework can be explored in more detail by reading the [official documentation](https://control.ros.org/master/doc/getting_started/getting_started.html), this [article](https://masum919.github.io/ros2_control_explained/) from Masum, this [tutorial](https://articulatedrobotics.xyz/tutorials/mobile-robot/applications/ros2_control-concepts/) from Articulated Robotics and the [preprint](http://dx.doi.org/10.13140/RG.2.2.15748.54408) for this project.
## Test Drive
### Robot localization
Before setting up lidarbot for autonomous navigation with the Nav2 stack, its wheel encoder and IMU sensor data are fused. Slipping between the wheels and the ground is [common in differential drive robots](https://www.sciencedirect.com/science/article/pii/S1474667016428909), and can lead to substantial deviation from the actual position of a wheeled mobile robot from its desired position. Fusing different sensor data aids in correcting for this variation. An Extended Kalman Filter is employed by the [`robot_localization`](https://index.ros.org/p/robot_localization/#humble) package to fuse the wheel odometry and IMU sensor data to obtain more accurate robot odometry estimates.
This results in improved robot navigation as the Nav2 stack makes use of odometry data to estimate the pose (position and orientation) of the robot. The wheel odometry and IMU sensor data are obtained from the differential drive controller topic, `diff_controller/odom`, and the IMU sensor broadcaster topic, `imu_broadcaster/imu` respectively.
The EKF is configured using this [guide](https://docs.nav2.org/setup_guides/odom/setup_robot_localization.html) from Nav2; more information about the `robot_localization` package can be found in the [official documenation](https://docs.ros.org/en/noetic/api/robot_localization/html/configuring_robot_localization.html). The EKF configuration file, `ekf.yaml`, is available [here](./lidarbot_navigation/config/ekf.yaml).
By default the Gazebo and real world lidarbot bringup launch files have the `use_robot_localization` argument set to `true` but this can also be set to `false` if one chooses to do so.
```
ros2 launch lidarbot_bringup lidarbot_bringup_launch.py use_robot_localization:=false
```
### Gazebo
To drive lidarbot around in Gazebo Fortress, first ensure that the USB dongle of the gamepad is plugged in to the development machine then run this command to bringup lidarbot, sensors and also integrates `ros2_control`, `twist_mux`, `robot_localization` and gamepad/joystick control:
```
ros2 launch lidarbot_gz gz_launch.py
```
The GIF below shows lidarbot being driven around by the gamepad (with the gamepad configuration presented [here](#teleoperation)) in a [world made up of obstacles](https://articulatedrobotics.xyz/tutorials/mobile-robot/concept-design/concept-gazebo#making-an-obstacle-course).
<p align='center'>
<img src=docs/images/gazebo_test_drive.gif width="600">
</p>
### Physical
A [ROS service](https://foxglove.dev/blog/creating-ros2-services) was written to test the connections of the motor(s) and by extension to know if the motor is faulty. Before running the tests, ensure that the 18650 batteries are charged, then prop the robot on a box or similar to prevent it falling off an edge for instance.
Run the [client node](./lidarbot_base/src/motor_checks_client.cpp) to request the motor checks:
```
ros2 run lidarbot_base motor_checks_client
```
Then run the [server node](./lidarbot_base/src/motor_checks_server.cpp) to check the motors:
```
ros2 run lidarbot_base motor_checks_server
```
These are the steps followed by the server node:
- The server initializes the left and right motor pulse counts to 0.
- It then runs each motor in the forward direction at 50% speed for 2 seconds. The terminal output is shown below:
```
[INFO] [1699988646.067449176] [rclcpp]: Ready to check motors
[INFO] [1699988648.190017279] [rclcpp]: Received request to check motors...
[INFO] [1699988648.190117076] [rclcpp]: Checking left motor...
[INFO] [1699988652.196330587] [rclcpp]: Checking right motor...
[INFO] [1699988656.202619229] [rclcpp]: Sending back response...
```
- The current pulse counts for each motor are checked to confirm that the pulse is above 0.
- If both motors have their pulse counts above 0, a success message is sent to the client:
```
[INFO] [1699991078.643028687] [rclcpp]: service not available, waiting again...
[INFO] [1699991087.233641544] [rclcpp]: The checks were successful!
```
- If one or both of the motors do not have pulse counts above zero a warning message is sent from the server to the client and identifies the faulty motor(s). At the moment, however, the message sent to the client does not identify the faulty motor but instead outputs this message when there is an error:
```
terminate called after throwing an instance of 'std::future_error'
what(): std::future_error: No associated state
[ros2run]: Aborted
```
This section will be updated once the issue has been fixed.
<p align='center'>
<img src=docs/images/motor_connection_tests.gif width="400">
</p>
If a motor moves backward instead of forward, swap the cables for the specific motor to change the direction.
After it is confirmed that both motors moved forward, lidarbot can be driven around with the gamepad after running this command:
```
ros2 launch lidarbot_bringup lidarbot_bringup_launch.py
```
This launch file brings up the physical lidarbot, raspberry pi camera, RPLIDAR A1 and also integrates `ros2_control`, `twist_mux`, `robot_localization` and gamepad/joystick control.
**Note:** There are some warning and error messages outputted to the terminal related to the camera. These are mostly related to calibrating the camera and can be ignored.
## Mapping
The Nav2 stack uses laser scan data to build a map of an environment and to localize the robot in the map while in motion. The SLAM package, [`slam_toolbox`](https://joss.theoj.org/papers/10.21105/joss.02783.pdf), is used to generate a map of Lidarbot’s surroundings with the RPLIDAR A1 by driving the robot around using the [`teleop_twist_joy`](https://index.ros.org/r/teleop_twist_joy/) package with the [gamepad](./lidarbot_teleop/launch/joystick_launch.py).
### Gazebo
Before starting the mapping operation, ensure that the `mode` key, under `ROS Parameters` in the [`mapper_params_online_async.yaml`](./lidarbot_slam/config/mapper_params_online_async.yaml) file, is set to `mapping` and also that the `map_file_name`, `map_start_pose` and the `map_start_at_dock` keys are commented out:
```
# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_footprint
scan_topic: /scan
use_map_saver: true
mode: mapping #localization
# if you'd like to immediately start continuing a map at a given pose
# or at the dock, but they are mutually exclusive, if pose is given
# will use pose
#map_file_name: /path/to/map_file
#map_start_pose: [0.0, 0.0, 0.0]
#map_start_at_dock: true
```
To start mapping in a simulation environment, launch the Gazebo simulation of lidarbot on the development machine (which includes the joystick node for teleoperation):
```
ros2 launch lidarbot_gz gz_launch.py
```
For Gazebo Fortress run:
```
ros2 launch lidarbot_gazebo gazebo_launch.py
```
In a separate terminal, navigate to the workspace directory, `lidarbot_ws` for example, and launch `slam_toolbox` with the [`online_async_launch.py`](./lidarbot_slam/launch/online_async_launch.py) file:
```
ros2 launch lidarbot_slam online_async_launch.py \
slam_params_file:=src/lidarbot_slam/config/mapper_params_online_async.yaml \
use_sim_time:=true
```
***Note**: The online asynchronous mode in `slam_toolbox` uses live and the most recent scan data to create a map, to avoid any lags therefore, some scans can be skipped.*
In another terminal, navigate to the workspace directory again and start `rviz2` with the `lidarbot_slam.rviz` config file:
```
rviz2 -d src/lidarbot_slam/rviz/lidarbot_slam.rviz
```
Drive around the obstacles to generate a map of the environment:
<p align='center'>
<img src=docs/images/gazebo_mapping.gif width="800">
</p>
After generating the map, in the **SlamToolboxPlugin** in RViz, type in a name for the map in the field beside the **Save Map** button, then click on it.
<p align='center'>
<img src=docs/images/save_map.png width="400">
</p>
The saved map can be found in the workspace directory and will be used by [Nav2 stack](https://navigation.ros.org/) for navigation.
### Physical
To begin mapping in the real world, first run the bringup command:
```
ros2 launch lidarbot_bringup lidarbot_bringup_launch.py
```
Then ensure that the [`mapper_params_online_async.yaml`](./lidarbot_slam/config/mapper_params_online_async.yaml) file is configured for mapping (refer to the previous subsection). Then open a new terminal, on the development machine, navigate to the workspace directory and launch `slam_toolbox` with the `use_sim_time` parameter set to `false`:
```
ros2 launch lidarbot_slam online_async_launch.py \
slam_params_file:=src/lidarbot_slam/config/mapper_params_online_async.yaml \
use_sim_time:=false
```
In a new terminal, also on the development machine, navigate to the workspace directory again and start `rviz2`:
```
rviz2 -d src/lidarbot_slam/rviz/lidarbot_slam.rviz
```
Drive around the environment to generate a map:
<p align='center'>
<img src=docs/images/real_mapping.gif width="600">
</p>
Then save the generated map.
## Navigation
To localize a robot in a given map, Nav2 makes use of the Adaptive Monte Carlo Localization (AMCL) package, `nav2_amcl`. This a probabilistic localization module that outputs the pose estimates of a robot taking LIDAR scans, a map (from the Nav2 `map_server`) and transform messages as inputs.
It was noted in the [controllers subsection](#controllers) that the Nav2 stack outputs command velocity messages to the differential drive controller. These are sent on the topic `diff_controller/cmd_vel_unstamped` for lidarbot. The velocity messages are sent to the robot base system hardware component to drive the motors. The updated robot odometry data from this motion is sent to the EKF on the topic `/diff_controller/odom`, and fused with the IMU data from the `imu_broadcaster/imu` topic. The fused odometry data is input into Nav2, in addition to the laser scan data, occupancy grid map and transform messages, to output new velocity messages. This process keeps iterating as the robot moves towards the goal position.
The Nav2 topic graph is shown in the following image adapted from [Linorobot2](https://github.com/linorobot/linorobot2).
<p align='center'>
<img src=docs/images/nav2_topics.png width="800">
</p>
### Gazebo
Lidarbot will be localized and navigated within the map previously generated with the `slam_toolbox`.
Bring up lidarbot in Gazebo Fortress:
```
ros2 launch lidarbot_gz gz_launch.py
```
Bring up lidarbot in Gazebo classic:
```
ros2 launch lidarbot_gazebo gazebo_launch.py
```
Navigate to the development workspace and open the following terminals.
To open `RViz`:
```
rviz2 -d src/lidarbot_navigation/rviz/lidarbot_nav.rviz
```
To localize lidarbot in the map with `nav2_amcl`:
```
ros2 launch lidarbot_navigation localization_launch.py \
map:=./src/lidarbot_navigation/maps/sim_map.yaml use_sim_time:=true
```
To launch navigation:
```
ros2 launch lidarbot_navigation navigation_launch.py use_sim_time:=true \
map_subscribe_transient_local:=true
```
Make sure to set the `use_sim_time` argument to `true`.
In `RViz`, set the initial pose using the `2D Pose Estimate` button in the toolbar so that lidarbot is aligned correctly both in `RViz` and Gazebo Classic. Afterwards, click on the `2D Goal Pose` and choose a place on the map for lidarbot to navigate to:
<p align='center'>
<img src=docs/images/gazebo_navigation.gif width="800">
</p>
The blue arrows indicate unfiltered odometry, while the green ones are the filtered odometry.
### Physical
Run a similar set of commands on the physical lidarbot for localization and navigation. Bring up lidarbot:
```
ros2 launch lidarbot_bringup lidarbot_bringup_launch.py
```
Launch `RViz`:
```
rviz2 -d src/lidarbot_navigation/rviz/lidarbot_nav.rviz
```
To localize lidarbot in the map with `nav2_amcl`:
```
ros2 launch lidarbot_navigation localization_launch.py map:=./real_map.yaml use_sim_time:=false
```
To launch navigation:
```
ros2 launch lidarbot_navigation navigation_launch.py use_sim_time:=false \
map_subscribe_transient_local:=true
```
Make sure to set the `use_sim_time` argument to `false`.
Similarly, set the initial pose using the `2D Pose Estimate` button in the `RViz` toolbar so that lidarbot is aligned correctly both in `RViz` and in its environment. Afterwards, click on the `2D Goal Pose` and choose a place on the map for lidarbot to navigate to.
To save time, all of these commands can be called from one launch file and adding delays where necessary to ensure certain processes are launched before others to avoid errors. Additional information on setting up navigation with Nav2 is available in this [setup guide](https://docs.nav2.org/setup_guides/index.html).
An ArUco marker was placed on top of lidarbot to track its trajectory during navigation, a Logitech C270 webcam was utilized in tracking the marker.
## ArUco package
To track the trajectory of lidarbot during navigation, an ArUco marker, a binary synthetic square marker, was placed on top of it. Synthetic or fiducial markers are used in [computer vision applications](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html) for pose estimation in robot navigation, augmented reality and more.
The four corners of fiducial markers provide enough correspondence between points in the real world and their 2D image projection to obtain the camera pose estimate.
More information about ArUco markers can be found in [this](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html) OpenCV tutorial.
### Generate ArUco marker
The open source computer vision library, [OpenCV](https://docs.opencv.org/4.x/d1/dfb/intro.html), contains an ArUco module based on the [ArUco library](https://www.uco.es/investiga/grupos/ava/portfolio/aruco/), a popular library for detection of square fiducial markers developed by Rafael Muñoz and Sergio Garrido.
Firstly, the `opencv-contrib-python` module needs to be installed and not `opencv-python`:
```
pip uninstall opencv-python
pip install opencv-contrib-python
```
The computer might need to be restarted for the install to be effected.
Next navigate to the path in the `lidarbot_aruco` pacakge directory:
```
cd ~/dev_ws/lidarbot_aruco/lidarbot_aruco
```
Then run the following script:
```python
python generate_aruco_marker.py --id 24 --type DICT_4X4_50 \
--output ../tags/DICT_4X4_50_id24.png
```
The script arguments:
`--id` : The unique identifier of the ArUco tag — this is a required argument and ID must be a valid ID in the ArUco dictionary used to generate the tag
`--type` : The name of the ArUco dictionary used to generate the tag; the default type is `DICT_4X4_50`
`--output` : The path to the output image where the generated ArUco tag will be saved; this is a required argument
Running the previous script opens a window with the generated ArUco tag displayed,
<p align='center'>
<img src=docs/images/generated_aruco_marker.png width="400">
</p>
To close the window, press the **q** key on the keyboard on the opened window.
### Webcam calibration
The Logitech webcam C270 HD is used in this project and needs to be calibrated.
First install the [ROS usb camera driver](https://index.ros.org/r/usb_cam/#humble) package:
```
sudo apt install ros-humble-usb-cam
```
Camera calibration was done following the steps outlined this [guide](https://automaticaddison.com/how-to-perform-pose-estimation-using-an-aruco-marker/)
Execute the command below to run the usb-cam driver node:
```
ros2 run usb_cam usb_cam_node_exe --ros-args --params-file ~/dev_ws/src/lidarbot_aruco/config/params_1.yaml
```
### Aruco trajectory visualizer node
```
ros2 run lidarbot_aruco aruco_trajectory_visualizer_node
```
Launch file to bringup the usb driver and aruco trajectory visualizer node:
```
ros2 launch lidarbot_aruco trajectory_visualizer_launch.py
```
<p align='center'>
<img src=docs/images/lidarbot_aruco_marker.png width="600">
</p>
<p align='center'>
<img src=docs/images/lidarbot_aruco_test.gif width="800">
</p>
The following images show the start and end positions of navigation with lidarbot.
<p align='center'>
<img src=docs/images/nav_start.png width="400">
</p>
<p align='center'>
<img src=docs/images/nav_end.png width="400">
</p>
An alternative to using AruCo markers to visualize the robot's trajectory is to use [this](https://github.com/MOGI-ROS/mogi_trajectory_server) ROS2 trajectory visualization server by MOGI-ROS.
## Acknowledgment
- [Articulated Robotics](https://articulatedrobotics.xyz/)
- [Automatic Addison](https://automaticaddison.com/)
- [Diffbot](https://github.com/ros-mobile-robots/diffbot)
- [Linorobot2](https://github.com/linorobot/linorobot2)
- [Mini pupper](https://github.com/mangdangroboticsclub/mini_pupper_ros)
- [Pyimagesearch](https://pyimagesearch.com/)
- [Robotics Backend](https://roboticsbackend.com/)
================================================
FILE: docs/index.md
================================================
================================================
FILE: lidarbot/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 3.8)
project(lidarbot)
find_package(ament_cmake REQUIRED)
ament_package()
================================================
FILE: lidarbot/package.xml
================================================
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>lidarbot</name>
<version>0.0.0</version>
<description>Lidarbot meta package</description>
<maintainer email="noobinventor@tutamail.com">Chinedu Amadi</maintainer>
<license>BSD 3-Clause</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
================================================
FILE: lidarbot_aruco/config/chessboard_calibration.yaml
================================================
%YAML:1.0
---
image_width: 640
image_height: 480
camera_name: narrow_stereo
camera_matrix:
rows: 3
cols: 3
dt: d
data: [842.57825, 0. , 304.89032,
0. , 841.64125, 224.5223 ,
0. , 0. , 1. ]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
dt: d
data: [0.002363, 0.562980, -0.004719, -0.008675, 0.000000]
rectification_matrix:
rows: 3
cols: 3
data: [1., 0., 0.,
0., 1., 0.,
0., 0., 1.]
projection_matrix:
rows: 3
cols: 4
data: [862.94263, 0. , 302.0357 , 0. ,
0. , 863.9823 , 223.20963, 0. ,
0. , 0. , 1. , 0. ]
================================================
FILE: lidarbot_aruco/config/params_1.yaml
================================================
/**:
ros__parameters:
video_device: "/dev/video0"
framerate: 30.0
io_method: "mmap"
frame_id: "camera"
pixel_format: "mjpeg2rgb" # see usb_cam/supported_formats for list of supported formats
av_device_format: "YUV422P"
image_width: 640
image_height: 480
camera_name: "test_camera"
camera_info_url: "package://usb_cam/config/camera_info.yaml"
brightness: -1
contrast: -1
saturation: -1
sharpness: -1
gain: -1
auto_white_balance: true
white_balance: 4000
autoexposure: true
exposure: 100
autofocus: false
focus: -1
================================================
FILE: lidarbot_aruco/config/params_2.yaml
================================================
/**:
ros__parameters:
video_device: "/dev/video2"
framerate: 15.0
io_method: "mmap"
frame_id: "camera2"
pixel_format: "mjpeg2rgb"
av_device_format: "YUV422P"
image_width: 640
image_height: 480
camera_name: "test_camera2"
# reusing same camera intrinsics only for demo, should really be unique for camera2"
camera_info_url: "package://usb_cam/config/camera_info.yaml"
brightness: -1
contrast: -1
saturation: -1
sharpness: -1
gain: -1
auto_white_balance: true
white_balance: 4000
autoexposure: true
exposure: 100
autofocus: false
focus: -1
================================================
FILE: lidarbot_aruco/launch/trajectory_visualizer_launch.py
================================================
# Launch file to start the webcam usb driver and aruco trajectory visualizer node
# to track the robot as it moves
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
usb_cam_params = os.path.join(
get_package_share_directory("lidarbot_aruco"), "config", "params_1.yaml"
)
usb_cam_node = Node(
package="usb_cam",
executable="usb_cam_node_exe",
parameters=[usb_cam_params],
)
aruco_visualizer_node = Node(
package="lidarbot_aruco",
executable="aruco_trajectory_visualizer_node",
output="screen",
)
return LaunchDescription(
[
usb_cam_node,
aruco_visualizer_node,
]
)
================================================
FILE: lidarbot_aruco/lidarbot_aruco/__init__.py
================================================
================================================
FILE: lidarbot_aruco/lidarbot_aruco/aruco_trajectory_visualizer.py
================================================
#!/usr/bin/env python3
# This node is used to visualize the trajectory of lidarbot by tracking the ArUco marker
# on it and drawing lines through the center of the marker in subsequent image frames
# Adapted from: https://automaticaddison.com/estimate-aruco-marker-pose-using-opencv-gazebo-and-ros-2/
# Import Python libraries
import cv2
import numpy as np
# Import ROS2 libraries
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data # Uses Best Effort reliability for camera
from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
from sensor_msgs.msg import Image # Image is the message type
# The different ArUco dictionaries built into the OpenCV library
ARUCO_DICT = {
"DICT_4X4_50": cv2.aruco.DICT_4X4_50,
"DICT_4X4_100": cv2.aruco.DICT_4X4_100,
"DICT_4X4_250": cv2.aruco.DICT_4X4_250,
"DICT_4X4_1000": cv2.aruco.DICT_4X4_1000,
"DICT_5X5_50": cv2.aruco.DICT_5X5_50,
"DICT_5X5_100": cv2.aruco.DICT_5X5_100,
"DICT_5X5_250": cv2.aruco.DICT_5X5_250,
"DICT_5X5_1000": cv2.aruco.DICT_5X5_1000,
"DICT_6X6_50": cv2.aruco.DICT_6X6_50,
"DICT_6X6_100": cv2.aruco.DICT_6X6_100,
"DICT_6X6_250": cv2.aruco.DICT_6X6_250,
"DICT_6X6_1000": cv2.aruco.DICT_6X6_1000,
"DICT_7X7_50": cv2.aruco.DICT_7X7_50,
"DICT_7X7_100": cv2.aruco.DICT_7X7_100,
"DICT_7X7_250": cv2.aruco.DICT_7X7_250,
"DICT_7X7_1000": cv2.aruco.DICT_7X7_1000,
"DICT_ARUCO_ORIGINAL": cv2.aruco.DICT_ARUCO_ORIGINAL,
}
class ArucoNode(Node):
def __init__(self):
super().__init__("aruco_visualizer_node")
# Declare parameters
self.declare_parameter("aruco_dictionary_name", "DICT_4X4_50")
self.declare_parameter("aruco_marker_side_length", 0.063)
self.declare_parameter(
"camera_calibration_parameters_filename",
"/home/noobinventor/lidarbot_ws/src/lidarbot_aruco/config/chessboard_calibration.yaml",
)
self.declare_parameter("image_topic", "/image_raw")
# Read parameters
aruco_dictionary_name = (
self.get_parameter("aruco_dictionary_name")
.get_parameter_value()
.string_value
)
self.aruco_marker_side_length = (
self.get_parameter("aruco_marker_side_length")
.get_parameter_value()
.double_value
)
self.camera_calibration_parameters_filename = (
self.get_parameter("camera_calibration_parameters_filename")
.get_parameter_value()
.string_value
)
image_topic = (
self.get_parameter("image_topic").get_parameter_value().string_value
)
# Check that we have a valid ArUco marker
if ARUCO_DICT.get(aruco_dictionary_name, None) is None:
self.get_logger().info(
"[INFO] ArUCo tag of '{}' is not supported".format(
aruco_dictionary_name
)
)
# Load the camera parameters from the saved file
cv_file = cv2.FileStorage(
self.camera_calibration_parameters_filename, cv2.FILE_STORAGE_READ
)
self.camera_matrix = cv_file.getNode("camera_matrix").mat()
self.dist_coeffs = cv_file.getNode("distortion_coefficients").mat()
cv_file.release()
# Load the ArUco dictionary
self.get_logger().info(
"[INFO] detecting '{}' markers...".format(aruco_dictionary_name)
)
self.arucoDict = cv2.aruco.getPredefinedDictionary(
ARUCO_DICT[aruco_dictionary_name]
)
self.arucoParams = cv2.aruco.DetectorParameters()
# Create the subscriber. This subscriber will receive an Image
# from the image_topic
self.subscription = self.create_subscription(
Image,
image_topic,
self.listener_callback,
qos_profile=qos_profile_sensor_data,
)
self.subscription # prevent unused variable warning
# Used to convert between ROS and OpenCV images
self.bridge = CvBridge()
# List of points to draw curves through
self.points = []
# Callback function
def listener_callback(self, data):
# Display the message on the console
self.get_logger().info("Receiving video frame")
# Convert ROS image message to OpenCV image
current_frame = self.bridge.imgmsg_to_cv2(data, desired_encoding="bgr8")
# Detect ArUco markers in the video frame
(corners, marker_ids, rejected) = cv2.aruco.detectMarkers(
current_frame,
self.arucoDict,
parameters=self.arucoParams,
)
# Check that at least one ArUco marker was detected
if marker_ids is not None:
# Get the rotation and translation vectors
rvecs, tvecs, obj_points = cv2.aruco.estimatePoseSingleMarkers(
corners,
self.aruco_marker_side_length,
self.camera_matrix,
self.dist_coeffs,
)
# Loop over the detected ArUCo corners
for markerCorner, markerID in zip(corners, marker_ids):
# Extract the marker corners (which are always returned in
# top-left, top-right, bottom-right, and bottom-left order)
corners = markerCorner.reshape((4, 2))
(topLeft, topRight, bottomRight, bottomLeft) = corners
# Convert the (x,y) coordinates pairs to integers
topRight = (int(topRight[0]), int(topRight[1]))
bottomRight = (int(bottomRight[0]), int(bottomRight[1]))
bottomLeft = (int(bottomLeft[0]), int(bottomLeft[1]))
topLeft = (int(topLeft[0]), int(topLeft[1]))
# Draw the bounding box of the ArUCo detection
cv2.line(current_frame, topLeft, topRight, (0, 255, 0), 2)
cv2.line(current_frame, topRight, bottomRight, (0, 255, 0), 2)
cv2.line(current_frame, bottomRight, bottomLeft, (0, 255, 0), 2)
cv2.line(current_frame, bottomLeft, topLeft, (0, 255, 0), 2)
# Compute and draw the center (x, y)-coordinates of the ArUco
# marker
cX = int((topLeft[0] + bottomRight[0]) / 2.0)
cY = int((topLeft[1] + bottomRight[1]) / 2.0)
self.points.append([cX, cY])
cv2.circle(current_frame, (cX, cY), 4, (0, 0, 255), -1)
# Convert points list to numpy array
points_array = np.array(self.points)
points_array = points_array.reshape((-1, 1, 2))
# Connect center coordinates of detected marker in respective frames
# NOTE: this section works for only one marker
cv2.polylines(
current_frame, [points_array], False, (0, 255, 0), thickness=5
)
# Display image for testing
cv2.imshow("camera", current_frame)
cv2.waitKey(1)
def main(args=None):
# Initialize the rclpy library
rclpy.init(args=args)
# Create the node
aruco_visualizer_node = ArucoNode()
# Spin the node so the callback function is called
rclpy.spin(aruco_visualizer_node)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
aruco_visualizer_node.destroy_node()
# Shutdown the ROS client library for Python
rclpy.shutdown()
if __name__ == "__main__":
main()
================================================
FILE: lidarbot_aruco/lidarbot_aruco/detect_aruco_marker.py
================================================
#!/usr/bin/env python
# This detects ArUco markers from a webcam stream using OpenCV and Python
# Adapted from https://automaticaddison.com/how-to-detect-aruco-markers-using-opencv-and-python/
# and https://pyimagesearch.com/2020/12/21/detecting-aruco-markers-with-opencv-and-python/
import argparse
import cv2
import sys
# Construct argument parser and parse the arguments
ap = argparse.ArgumentParser()
ap.add_argument(
"-t", "--type", type=str, default="DICT_4X4_50", help="type of ArUco tag to detect"
)
args = vars(ap.parse_args())
# The different ArUco dictinaries built into OpenCV
ARUCO_DICT = {
"DICT_4X4_50": cv2.aruco.DICT_4X4_50,
"DICT_4X4_100": cv2.aruco.DICT_4X4_100,
"DICT_4X4_250": cv2.aruco.DICT_4X4_250,
"DICT_4X4_1000": cv2.aruco.DICT_4X4_1000,
"DICT_5X5_50": cv2.aruco.DICT_5X5_50,
"DICT_5X5_100": cv2.aruco.DICT_5X5_100,
"DICT_5X5_250": cv2.aruco.DICT_5X5_250,
"DICT_5X5_1000": cv2.aruco.DICT_5X5_1000,
"DICT_6X6_50": cv2.aruco.DICT_6X6_50,
"DICT_6X6_100": cv2.aruco.DICT_6X6_100,
"DICT_6X6_250": cv2.aruco.DICT_6X6_250,
"DICT_6X6_1000": cv2.aruco.DICT_6X6_1000,
"DICT_7X7_50": cv2.aruco.DICT_7X7_50,
"DICT_7X7_100": cv2.aruco.DICT_7X7_100,
"DICT_7X7_250": cv2.aruco.DICT_7X7_250,
"DICT_7X7_1000": cv2.aruco.DICT_7X7_1000,
"DICT_ARUCO_ORIGINAL": cv2.aruco.DICT_ARUCO_ORIGINAL,
}
# Main method
def main():
# Check that we have a valid ArUco marker
if ARUCO_DICT.get(args["type"], None) is None:
print("[INFO] ArUco tag of '{}' is not supported".format(args["type"]))
sys.exit(0)
# Load the ArUco dictionary
print("[INFO] detecting '{}' tags...".format(args["type"]))
arucoDict = cv2.aruco.getPredefinedDictionary(ARUCO_DICT[args["type"]])
arucoParams = cv2.aruco.DetectorParameters()
# Start the video stream
cap = cv2.VideoCapture(0)
while True:
# Capture frame-by-frame
# This method returns True/False as well as the video frame
ret, frame = cap.read()
# Detect ArUco markers in the video frame
(corners, ids, rejected) = cv2.aruco.detectMarkers(
frame, arucoDict, parameters=arucoParams
)
# Check that at least one ArUco marker was detected
if len(corners) > 0:
# Flatten the ArUco IDs list of lists to a list
ids = ids.flatten()
# Loop over the detected ArUCo corners
for markerCorner, markerID in zip(corners, ids):
# Extract the marker corners (which are always returned in
# top-left, top-right, bottom-right, and bottom-left order)
corners = markerCorner.reshape((4, 2))
(topLeft, topRight, bottomRight, bottomLeft) = corners
# Convert the (x,y) coordinates pairs to integers
topRight = (int(topRight[0]), int(topRight[1]))
bottomRight = (int(bottomRight[0]), int(bottomRight[1]))
bottomLeft = (int(bottomLeft[0]), int(bottomLeft[1]))
topLeft = (int(topLeft[0]), int(topLeft[1]))
# Draw the bounding box of the ArUCo detection
cv2.line(frame, topLeft, topRight, (0, 255, 0), 2)
cv2.line(frame, topRight, bottomRight, (0, 255, 0), 2)
cv2.line(frame, bottomRight, bottomLeft, (0, 255, 0), 2)
cv2.line(frame, bottomLeft, topLeft, (0, 255, 0), 2)
# Compute and draw the center (x, y)-coordinates of the ArUco
# marker
cX = int((topLeft[0] + bottomRight[0]) / 2.0)
cY = int((topLeft[1] + bottomRight[1]) / 2.0)
cv2.circle(frame, (cX, cY), 4, (0, 0, 255), -1)
# Draw the ArUco marker ID on the image
cv2.putText(
frame,
str(markerID),
(topLeft[0], topLeft[1] - 15),
cv2.FONT_HERSHEY_SIMPLEX,
0.5,
(0, 255, 0),
2,
)
print("[INFO] ArUco marker ID: {}".format(markerID))
# Display the resulting frame
cv2.imshow("frame", frame)
# If "q" is pressed on the keyboard,
# exit this loop
if cv2.waitKey(1) & 0xFF == ord("q"):
break
# Close down the video stream
cap.release()
cv2.destroyAllWindows()
if __name__ == "__main__":
main()
================================================
FILE: lidarbot_aruco/lidarbot_aruco/generate_aruco_marker.py
================================================
#!/usr/bin/env python
# This script generates ArUco markers using OpenCV and Python
# Adapted from https://automaticaddison.com/how-to-create-an-aruco-marker-using-opencv-python/
# and https://pyimagesearch.com/2020/12/14/generating-aruco-markers-with-opencv-and-python/
import cv2
import argparse
import numpy as np
import sys
# Construct the argument parser and parse the arguments
ap = argparse.ArgumentParser()
ap.add_argument(
"-o", "--output", required=True, help="path to output image containing ArUCo tag"
)
ap.add_argument(
"-i",
"--id",
type=int,
required=True,
help="ID of ArUCo tag to generate",
)
ap.add_argument(
"-t",
"--type",
type=str,
default="DICT_4X4_50",
help="type of ArUCo tag to generate",
)
args = vars(ap.parse_args())
# The different ArUco dictinaries built into OpenCV
ARUCO_DICT = {
"DICT_4X4_50": cv2.aruco.DICT_4X4_50,
"DICT_4X4_100": cv2.aruco.DICT_4X4_100,
"DICT_4X4_250": cv2.aruco.DICT_4X4_250,
"DICT_4X4_1000": cv2.aruco.DICT_4X4_1000,
"DICT_5X5_50": cv2.aruco.DICT_5X5_50,
"DICT_5X5_100": cv2.aruco.DICT_5X5_100,
"DICT_5X5_250": cv2.aruco.DICT_5X5_250,
"DICT_5X5_1000": cv2.aruco.DICT_5X5_1000,
"DICT_6X6_50": cv2.aruco.DICT_6X6_50,
"DICT_6X6_100": cv2.aruco.DICT_6X6_100,
"DICT_6X6_250": cv2.aruco.DICT_6X6_250,
"DICT_6X6_1000": cv2.aruco.DICT_6X6_1000,
"DICT_7X7_50": cv2.aruco.DICT_7X7_50,
"DICT_7X7_100": cv2.aruco.DICT_7X7_100,
"DICT_7X7_250": cv2.aruco.DICT_7X7_250,
"DICT_7X7_1000": cv2.aruco.DICT_7X7_1000,
"DICT_ARUCO_ORIGINAL": cv2.aruco.DICT_ARUCO_ORIGINAL,
}
# Main method
def main():
# Check that we have a valid ArUco marker
if ARUCO_DICT.get(args["type"], None) is None:
print("[INFO] ArUco tag of '{}' is not supported".format(args["type"]))
sys.exit(0)
# Load the ArUco dictionary
arucoDict = cv2.aruco.getPredefinedDictionary(ARUCO_DICT[args["type"]])
# Allocate memory for the output ArUCo tag and then draw the ArUCo
# tag on the output image
print(
"[INFO] generating ArUCo tag type '{}' with ID '{}'".format(
args["type"], args["id"]
)
)
tag = np.zeros((180, 180, 1), dtype="uint8")
cv2.aruco.generateImageMarker(arucoDict, args["id"], 180, tag, 1)
# Write the generated ArUCo tag to disk and then display it to our
# screen
cv2.imwrite(args["output"], tag)
cv2.imshow("ArUCo Tag", tag)
# If "q" is pressed on the keyboard on the opened window with the aruco tag
# close the window
if cv2.waitKey(0) & 0xFF == ord("q"):
cv2.destroyAllWindows
if __name__ == "__main__":
main()
================================================
FILE: lidarbot_aruco/package.xml
================================================
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>lidarbot_aruco</name>
<version>0.0.0</version>
<description>Lidarbot aruco package</description>
<maintainer email="noobinventor@tutamail.com">Chinedu Amadi</maintainer>
<license>BSD 3-Clause</license>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<depend>usb-cam</depend>
<depend>geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>tf_transformations</depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
================================================
FILE: lidarbot_aruco/resource/lidarbot_aruco
================================================
================================================
FILE: lidarbot_aruco/setup.cfg
================================================
[develop]
script_dir=$base/lib/lidarbot_aruco
[install]
install_scripts=$base/lib/lidarbot_aruco
================================================
FILE: lidarbot_aruco/setup.py
================================================
import os
from glob import glob
from setuptools import find_packages, setup
package_name = "lidarbot_aruco"
setup(
name=package_name,
version="0.0.0",
packages=find_packages(exclude=["test"]),
data_files=[
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
("share/" + package_name, ["package.xml"]),
(
os.path.join("share", package_name, "launch"),
glob(os.path.join("launch", "*launch.[pxy][yma]*")),
),
(
os.path.join("share", package_name, "config"),
glob(os.path.join("config", "*yaml")),
),
],
install_requires=["setuptools"],
zip_safe=True,
maintainer="Chinedu Amadi",
maintainer_email="noobinventor@tutamail.com",
description="TODO: Package description",
license="TODO: License declaration",
tests_require=["pytest"],
entry_points={
"console_scripts": [
"aruco_trajectory_visualizer_node = lidarbot_aruco.aruco_trajectory_visualizer:main"
],
},
)
================================================
FILE: lidarbot_aruco/test/test_copyright.py
================================================
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_copyright.main import main
import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found errors'
================================================
FILE: lidarbot_aruco/test/test_flake8.py
================================================
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_flake8.main import main_with_errors
import pytest
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)
================================================
FILE: lidarbot_aruco/test/test_pep257.py
================================================
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_pep257.main import main
import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found code style errors / warnings'
================================================
FILE: lidarbot_base/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 3.8)
project(lidarbot_base)
find_package(ament_cmake REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(std_srvs REQUIRED)
# COMPILE
add_library(lidarbot_hardware SHARED
src/lidarbot_hardware.cpp
src/wheel.cpp
src/MotorDriver.c
src/motor_encoder.c
src/PCA9685.c
src/DEV_Config.c
)
target_include_directories(lidarbot_hardware PRIVATE include)
ament_target_dependencies(
lidarbot_hardware
hardware_interface
pluginlib
rclcpp
rclcpp_lifecycle
)
# Link WiringPi library to lidarbot_hardware target
target_link_libraries(lidarbot_hardware wiringPi)
# Export hardware plugin
pluginlib_export_plugin_description_file(hardware_interface lidarbot_hardware.xml)
# INSTALL
install(
DIRECTORY include/
DESTINATION include/lidarbot_hardware
)
install(
TARGETS lidarbot_hardware
DESTINATION share/${PROJECT_NAME}
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}
)
include_directories(include)
# Add motor_checks_client and motor_checks_server nodes as executables with dependencies
add_executable(motor_checks_server
src/motor_checks_server.cpp
src/motor_encoder.c
src/DEV_Config.c
src/PCA9685.c
src/MotorDriver.c)
ament_target_dependencies(motor_checks_server rclcpp std_srvs)
target_link_libraries(motor_checks_server m wiringPi pthread crypt rt)
target_compile_options(motor_checks_server PRIVATE -w)
add_executable(motor_checks_client src/motor_checks_client.cpp)
ament_target_dependencies(motor_checks_client rclcpp std_srvs)
# Install executables to enable discovery for 'ros2 run'
install(TARGETS
motor_checks_client
motor_checks_server
DESTINATION lib/${PROJECT_NAME}
)
# EXPORTS
ament_export_libraries(lidarbot_hardware)
ament_package()
================================================
FILE: lidarbot_base/include/lidarbot_base/DEV_Config.h
================================================
#ifndef _DEV_CONFIG_H_
#define _DEV_CONFIG_H_
#define USE_WIRINGPI_LIB 1
/***********************************************************************************************************************
------------------------------------------------------------------------
|\\\ ///|
|\\\ Hardware interface ///|
------------------------------------------------------------------------
***********************************************************************************************************************/
#ifdef USE_BCM2835_LIB
#include <bcm2835.h>
#elif USE_WIRINGPI_LIB
#include <wiringPi.h>
#include <wiringPiI2C.h>
#elif USE_DEV_LIB
#include "sysfs_gpio.h"
#include "dev_hardware_i2c.h"
// #include "dev_hardware_SPI.h"
#endif
#include <stdint.h>
#include "Debug.h"
#include <errno.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#define DEV_SPI 0
#define DEV_I2C 1
/**
* data
**/
#define UBYTE uint8_t
#define UWORD uint16_t
#define UDOUBLE uint32_t
extern int INT_PIN;//4
/*------------------------------------------------------------------------------------------------------*/
uint8_t DEV_ModuleInit(void);
void DEV_ModuleExit(void);
void DEV_I2C_Init(uint8_t Add);
void I2C_Write_Byte(uint8_t Cmd, uint8_t value);
int I2C_Read_Byte(uint8_t Cmd);
int I2C_Read_Word(uint8_t Cmd);
void DEV_GPIO_Mode(UWORD Pin, UWORD Mode);
void DEV_Digital_Write(UWORD Pin, UBYTE Value);
UBYTE DEV_Digital_Read(UWORD Pin);
void DEV_Delay_ms(UDOUBLE xms);
void DEV_SPI_WriteByte(UBYTE Value);
void DEV_SPI_Write_nByte(uint8_t *pData, uint32_t Len);
#endif
================================================
FILE: lidarbot_base/include/lidarbot_base/Debug.h
================================================
/*****************************************************************************
* | File : Debug.h
* | Author : Waveshare team
* | Function : debug with printf
* | Info :
*----------------
* | This version: V1.0
* | Date : 2018-01-11
* | Info : Basic version
*
******************************************************************************/
#ifndef __DEBUG_H
#define __DEBUG_H
#include <stdio.h>
#define USE_DEBUG 0
#if USE_DEBUG
#define DEBUG(__info,...) printf("Debug : " __info,##__VA_ARGS__)
#else
#define DEBUG(__info,...)
#endif
#endif
================================================
FILE: lidarbot_base/include/lidarbot_base/MotorDriver.h
================================================
/*****************************************************************************
* | File : MotorDriver.h
* | Author : Waveshare team
* | Function : Drive TB6612FNG
* | Info :
* TB6612FNG is a driver IC for DC motor with output transistor in
* LD MOS structure with low ON-resistor. Two input signals, IN1
* and IN2, can choose one of four modes such as CW, CCW, short
* brake, and stop mode.
*----------------
* | This version: V1.0
* | Date : 2018-09-04
* | Info : Basic version
*
******************************************************************************/
#ifndef __TB6612FNG_
#define __TB6612FNG_
#include "DEV_Config.h"
#include "PCA9685.h"
//GPIO config
#define PWMA PCA_CHANNEL_0
#define AIN1 PCA_CHANNEL_1
#define AIN2 PCA_CHANNEL_2
#define PWMB PCA_CHANNEL_5
#define BIN1 PCA_CHANNEL_3
#define BIN2 PCA_CHANNEL_4
#define MOTORA 0
#define MOTORB 1
typedef enum {
BACKWARD = 0,
FORWARD,
} DIR;
void Motor_Init(void);
void Motor_Run(UBYTE motor, DIR dir, UWORD speed);
void Motor_Stop(UBYTE motor);
UBYTE Motor_Direction(UBYTE motor);
#endif
================================================
FILE: lidarbot_base/include/lidarbot_base/PCA9685.h
================================================
/*****************************************************************************
* | File : PCA9685.h
* | Author : Waveshare team
* | Function : Drive PCA9685
* | Info :
* Used to shield the underlying layers of each master
* and enhance portability
*----------------
* | This version: V1.0
* | Date : 2018-09-04
* | Info : Basic version
*
******************************************************************************/
#ifndef __PCA9685_H_
#define __PCA9685_H_
#include "DEV_Config.h"
//GPIO config
#define SUBADR1 0x02
#define SUBADR2 0x03
#define SUBADR3 0x04
#define MODE1 0x00
#define PRESCALE 0xFE
#define LED0_ON_L 0x06
#define LED0_ON_H 0x07
#define LED0_OFF_L 0x08
#define LED0_OFF_H 0x09
#define ALLLED_ON_L 0xFA
#define ALLLED_ON_H 0xFB
#define ALLLED_OFF_L 0xFC
#define ALLLED_OFF_H 0xFD
#define PCA_CHANNEL_0 0
#define PCA_CHANNEL_1 1
#define PCA_CHANNEL_2 2
#define PCA_CHANNEL_3 3
#define PCA_CHANNEL_4 4
#define PCA_CHANNEL_5 5
#define PCA_CHANNEL_6 6
#define PCA_CHANNEL_7 7
#define PCA_CHANNEL_8 8
#define PCA_CHANNEL_9 9
#define PCA_CHANNEL_10 10
#define PCA_CHANNEL_11 11
#define PCA_CHANNEL_12 12
#define PCA_CHANNEL_13 13
#define PCA_CHANNEL_14 14
#define PCA_CHANNEL_15 15
void PCA9685_Init(char addr);
void PCA9685_SetPWMFreq(UWORD freq);
void PCA9685_SetPwmDutyCycle(UBYTE channel, UWORD pulse);
void PCA9685_SetLevel(UBYTE channel, UWORD value);
#endif
================================================
FILE: lidarbot_base/include/lidarbot_base/lidarbot_hardware.hpp
================================================
#ifndef _LIDARBOT_BASE__LIDARBOT_HARDWARE_HPP_
#define _LIDARBOT_BASE__LIDARBOT_HARDWARE_HPP_
#include <cmath>
#include <memory>
#include <cstring>
#include <thread>
#include <vector>
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "motor_encoder.h"
#include "wheel.hpp"
using hardware_interface::return_type;
using hardware_interface::CallbackReturn;
namespace lidarbot_base
{
class LidarbotHardware : public hardware_interface::SystemInterface
{
struct Config
{
std::string left_wheel_name = "left_wheel";
std::string right_wheel_name = "right_wheel";
int enc_ticks_per_rev = 1084;
double loop_rate = 30.0;
};
public:
LidarbotHardware();
CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
private:
Config config_;
Wheel left_wheel_;
Wheel right_wheel_;
rclcpp::Logger logger_;
};
} // namespace lidarbot_base
#endif
================================================
FILE: lidarbot_base/include/lidarbot_base/motor_encoder.h
================================================
#ifdef __cplusplus
extern "C" {
#endif
#ifndef __MOTOR_ENCODER_H__
#define __MOTOR_ENCODER_H__
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <wiringPi.h>
#include "MotorDriver.h"
#define LEFT_WHL_ENC_INT 25
#define RIGHT_WHL_ENC_INT 24
#define LEFT_WHL_ENC_DIR 22
#define RIGHT_WHL_ENC_DIR 23
void handler(int signo);
void left_wheel_pulse();
void right_wheel_pulse();
void set_motor_speeds(double left_wheel_command, double right_wheel_command);
void read_encoder_values(int *left_encoder_value, int *right_encoder_value);
extern int left_wheel_pulse_count;
extern int right_wheel_pulse_count;
extern int left_wheel_direction;
extern int right_wheel_direction;
#endif
#ifdef __cplusplus
}
#endif
================================================
FILE: lidarbot_base/include/lidarbot_base/wheel.hpp
================================================
#ifndef _LIDARBOT_BASE__WHEEL_H_
#define _LIDARBOT_BASE__WHEEL_H_
#include <string>
#include <cmath>
class Wheel
{
public:
std::string name = "";
int encoder_ticks = 0;
double command = 0.0;
double position = 0.0;
double velocity = 0.0;
double rads_per_tick = 0.0;
Wheel() = default;
Wheel(const std::string &wheel_name, int ticks_per_rev);
void setup(const std::string &wheel_name, int ticks_per_rev);
double calculate_encoder_angle();
};
#endif
================================================
FILE: lidarbot_base/lidarbot_hardware.xml
================================================
<library path="lidarbot_hardware">
<class name="lidarbot_base/LidarbotHardware"
type="lidarbot_base::LidarbotHardware"
base_class_type="hardware_interface::SystemInterface">
<description>
The ros2_control hardware interface for the lidarbot that uses the PCA9685 chip (on the Motor Driver HAT) to adjust motor speeds.
</description>
</class>
</library>
================================================
FILE: lidarbot_base/package.xml
================================================
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>lidarbot_base</name>
<version>0.0.0</version>
<description>Lidarbot base motor driver package</description>
<maintainer email="noobinventor@tutamail.com">Chinedu Amadi</maintainer>
<license>BSD 3-Clause</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>controller_manager</depend>
<depend>hardware_interface</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<depend>ros2_control</depend>
<depend>ros2_controllers</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
================================================
FILE: lidarbot_base/src/DEV_Config.c
================================================
/*****************************************************************************
* | File : DEV_Config.c
* | Author : Waveshare team
* | Function : Hardware underlying interface
* | Info :
*----------------
* | This version: V2.0
* | Date : 2019-07-08
* | Info : Basic version
*
******************************************************************************/
#include "lidarbot_base/DEV_Config.h"
#include <unistd.h>
#include <fcntl.h>
uint32_t fd;
int INT_PIN;
/******************************************************************************
function: Equipment Testing
parameter:
Info: Only supports Jetson Nano and Raspberry Pi
******************************************************************************/
static int DEV_Equipment_Testing(void)
{
int i;
int fd;
char value_str[20];
fd = open("/etc/issue", O_RDONLY);
printf("Current environment: ");
while(1) {
if (fd < 0) {
return -1;
}
for(i=0;; i++) {
if (read(fd, &value_str[i], 1) < 0) {
return -1;
}
if(value_str[i] ==32) {
printf("\r\n");
break;
}
printf("%c",value_str[i]);
}
break;
}
if(i<5) {
printf("Unrecognizable\r\n");
return -1;
} else {
char RPI_System[10] = {"Raspbian"};
for(i=0; i<6; i++) {
if(RPI_System[i] != value_str[i]) {
#if USE_DEV_LIB
return 'J';
#endif
return -1;
}
}
return 'R';
}
return -1;
}
/******************************************************************************
function: GPIO Function initialization and transfer
parameter:
Info:
******************************************************************************/
void DEV_GPIO_Mode(UWORD Pin, UWORD Mode)
{
/*
0: INPT
1: OUTP
*/
#ifdef USE_BCM2835_LIB
if(Mode == 0 || Mode == BCM2835_GPIO_FSEL_INPT){
bcm2835_gpio_fsel(Pin, BCM2835_GPIO_FSEL_INPT);
}else {
bcm2835_gpio_fsel(Pin, BCM2835_GPIO_FSEL_OUTP);
}
#elif USE_WIRINGPI_LIB
if(Mode == 0 || Mode == INPUT){
pinMode(Pin, INPUT);
pullUpDnControl(Pin, PUD_UP);
}else{
pinMode(Pin, OUTPUT);
}
#elif USE_DEV_LIB
SYSFS_GPIO_Export(Pin);
if(Mode == 0 || Mode == SYSFS_GPIO_IN){
SYSFS_GPIO_Direction(Pin, SYSFS_GPIO_IN);
}else{
SYSFS_GPIO_Direction(Pin, SYSFS_GPIO_OUT);
}
#endif
}
void DEV_Digital_Write(UWORD Pin, UBYTE Value)
{
#ifdef USE_BCM2835_LIB
bcm2835_gpio_write(Pin, Value);
#elif USE_WIRINGPI_LIB
digitalWrite(Pin, Value);
#elif USE_DEV_LIB
SYSFS_GPIO_Write(Pin, Value);
#endif
}
UBYTE DEV_Digital_Read(UWORD Pin)
{
UBYTE Read_value = 0;
#ifdef USE_BCM2835_LIB
Read_value = bcm2835_gpio_lev(Pin);
#elif USE_WIRINGPI_LIB
Read_value = digitalRead(Pin);
#elif USE_DEV_LIB
Read_value = SYSFS_GPIO_Read(Pin);
#endif
return Read_value;
}
/**
* delay x ms
**/
void DEV_Delay_ms(UDOUBLE xms)
{
#ifdef USE_BCM2835_LIB
bcm2835_delay(xms);
#elif USE_WIRINGPI_LIB
delay(xms);
#elif USE_DEV_LIB
UDOUBLE i;
for(i=0; i < xms; i++){
usleep(1000);
}
#endif
}
void GPIO_Config(void)
{
int Equipment = DEV_Equipment_Testing();
if(Equipment=='R'){
INT_PIN = 4;
}else if(Equipment=='J'){
#if USE_DEV_LIB
INT_PIN = GPIO4;
#endif
}else{
printf("Device read failed or unrecognized!!!\r\n");
while(1);
}
DEV_GPIO_Mode(INT_PIN, 0);
}
/******************************************************************************
function: SPI Function initialization and transfer
parameter:
Info:
******************************************************************************/
void DEV_SPI_Init()
{
#if DEV_SPI
#ifdef USE_BCM2835_LIB
printf("BCM2835 SPI Device\r\n");
bcm2835_spi_begin(); //Start spi interface, set spi pin for the reuse function
bcm2835_spi_setBitOrder(BCM2835_SPI_BIT_ORDER_MSBFIRST); //High first transmission
bcm2835_spi_setDataMode(BCM2835_SPI_MODE0); //spi mode 0
bcm2835_spi_setClockDivider(BCM2835_SPI_CLOCK_DIVIDER_128); //Frequency
bcm2835_spi_chipSelect(BCM2835_SPI_CS0); //set CE0
bcm2835_spi_setChipSelectPolarity(BCM2835_SPI_CS0, LOW); //enable cs0
#elif USE_WIRINGPI_LIB
printf("WIRINGPI SPI Device\r\n");
//wiringPiSPISetup(0,9000000);
wiringPiSPISetupMode(0, 9000000, 0);
#elif USE_DEV_LIB
printf("DEV SPI Device\r\n");
DEV_HARDWARE_SPI_begin("/dev/spidev0.0");
#endif
#endif
}
void DEV_SPI_WriteByte(uint8_t Value)
{
#if DEV_SPI
#ifdef USE_BCM2835_LIB
bcm2835_spi_transfer(Value);
#elif USE_WIRINGPI_LIB
wiringPiSPIDataRW(0,&Value,1);
#elif USE_DEV_LIB
DEV_HARDWARE_SPI_TransferByte(Value);
#endif
#endif
}
void DEV_SPI_Write_nByte(uint8_t *pData, uint32_t Len)
{
#if DEV_SPI
#ifdef USE_BCM2835_LIB
uint8_t rData[Len];
bcm2835_spi_transfernb(pData,rData,Len);
#elif USE_WIRINGPI_LIB
wiringPiSPIDataRW(0, pData, Len);
#elif USE_DEV_LIB
DEV_HARDWARE_SPI_Transfer(pData, Len);
#endif
#endif
}
/******************************************************************************
function: I2C Function initialization and transfer
parameter:
Info:
******************************************************************************/
void DEV_I2C_Init(uint8_t Add)
{
#if DEV_I2C
#ifdef USE_BCM2835_LIB
printf("BCM2835 I2C Device\r\n");
bcm2835_i2c_begin();
bcm2835_i2c_setSlaveAddress(Add);
#elif USE_WIRINGPI_LIB
printf("WIRINGPI I2C Device\r\n");
fd = wiringPiI2CSetup(Add);
#elif USE_DEV_LIB
// printf("DEV I2C Device\r\n");
DEV_HARDWARE_I2C_begin("/dev/i2c-1");
DEV_HARDWARE_I2C_setSlaveAddress(Add);
#endif
#endif
}
void I2C_Write_Byte(uint8_t Cmd, uint8_t value)
{
int ref;
#if DEV_I2C
#ifdef USE_BCM2835_LIB
char wbuf[2]={Cmd, value};
bcm2835_i2c_write(wbuf, 2);
#elif USE_WIRINGPI_LIB
//wiringPiI2CWrite(fd,Cmd);
ref = wiringPiI2CWriteReg8(fd, (int)Cmd, (int)value);
while(ref != 0) {
ref = wiringPiI2CWriteReg8 (fd, (int)Cmd, (int)value);
if(ref == 0)
break;
}
#elif USE_DEV_LIB
char wbuf[2]={Cmd, value};
DEV_HARDWARE_I2C_write(wbuf, 2);
#endif
#endif
}
int I2C_Read_Byte(uint8_t Cmd)
{
int ref;
#if DEV_I2C
#ifdef USE_BCM2835_LIB
char rbuf[2]={0};
bcm2835_i2c_read_register_rs(&Cmd, rbuf, 1);
ref = rbuf[0];
#elif USE_WIRINGPI_LIB
ref = wiringPiI2CReadReg8 (fd, (int)Cmd);
#elif USE_DEV_LIB
char rbuf[2]={0};
DEV_HARDWARE_I2C_read(Cmd, rbuf, 1);
ref = rbuf[0];
#endif
#endif
return ref;
}
int I2C_Read_Word(uint8_t Cmd)
{
int ref;
#if DEV_I2C
#ifdef USE_BCM2835_LIB
char rbuf[2] = {0};
bcm2835_i2c_read_register_rs(&Cmd, rbuf, 2);
ref = rbuf[1]<<8 | rbuf[0];
#elif USE_WIRINGPI_LIB
ref = wiringPiI2CReadReg16 (fd, (int)Cmd);
#elif USE_DEV_LIB
char rbuf[2] = {0};
DEV_HARDWARE_I2C_read(Cmd, rbuf, 2);
ref = rbuf[1]<<8 | rbuf[0];
#endif
#endif
return ref;
}
/******************************************************************************
function: Module Initialize, the library and initialize the pins, SPI protocol
parameter:
Info:
******************************************************************************/
UBYTE DEV_ModuleInit(void)
{
#ifdef USE_BCM2835_LIB
if(!bcm2835_init()) {
printf("bcm2835 init failed !!! \r\n");
return 1;
} else {
printf("bcm2835 init success !!! \r\n");
}
#elif USE_WIRINGPI_LIB
//if(wiringPiSetup() < 0)//use wiringpi Pin number table
if(wiringPiSetupGpio() < 0) { //use BCM2835 Pin number table
printf("set wiringPi lib failed !!! \r\n");
return 1;
} else {
printf("set wiringPi lib success !!! \r\n");
}
#elif USE_DEV_LIB
printf("USE_DEV_LIB \r\n");
#endif
GPIO_Config();
DEV_I2C_Init(0x29);
return 0;
}
/******************************************************************************
function: Module exits, closes SPI and BCM2835 library
parameter:
Info:
******************************************************************************/
void DEV_ModuleExit(void)
{
#ifdef USE_BCM2835_LIB
#if DEV_I2C
bcm2835_i2c_end();
#endif
#if DEV_SPI
bcm2835_spi_end();
#endif
bcm2835_close();
#elif USE_WIRINGPI_LIB
#elif USE_DEV_LIB
#if DEV_I2C
DEV_HARDWARE_I2C_end();
#endif
#if DEV_SPI
DEV_HARDWARE_SPI_end();
#endif
#endif
}
================================================
FILE: lidarbot_base/src/MotorDriver.c
================================================
/*****************************************************************************
* | File : MotorDriver.c
* | Author : Waveshare team
* | Function : Drive TB6612FNG
* | Info :
* TB6612FNG is a driver IC for DC motor with output transistor in
* LD MOS structure with low ON-resistor. Two input signals, IN1
* and IN2, can choose one of four modes such as CW, CCW, short
* brake, and stop mode.
*----------------
* | This version: V1.0
* | Date : 2018-09-04
* | Info : Basic version
*
******************************************************************************/
#include "lidarbot_base/MotorDriver.h"
#include "lidarbot_base/Debug.h"
UWORD ain1_value, ain2_value;
UWORD bin1_value, bin2_value;
/**
* Motor rotation.
*
* Example:
* Motor_Init();
*/
void Motor_Init(void)
{
PCA9685_Init(0x40);
PCA9685_SetPWMFreq(50);
}
/**
* Motor rotation.
*
* @param motor: Motor A and Motor B.
* @param dir: forward and backward.
* @param speed: Rotation speed. //(0~100)
*
* Example:
* @code
* Motor_Run(MOTORA, FORWARD, 50);
* Motor_Run(MOTORB, BACKWARD, 100);
*/
void Motor_Run(UBYTE motor, DIR dir, UWORD speed)
{
if(speed > 100)
speed = 100;
if(motor == MOTORA) {
DEBUG("Motor A Speed = %d\r\n", speed);
PCA9685_SetPwmDutyCycle(PWMA, speed);
if(dir == FORWARD) {
DEBUG("forward...\r\n");
PCA9685_SetLevel(AIN1, 0);
PCA9685_SetLevel(AIN2, 1);
ain1_value = 0;
ain2_value = 1;
} else {
DEBUG("backward...\r\n");
PCA9685_SetLevel(AIN1, 1);
PCA9685_SetLevel(AIN2, 0);
ain1_value = 1;
ain2_value = 0;
}
} else {
DEBUG("Motor B Speed = %d\r\n", speed);
PCA9685_SetPwmDutyCycle(PWMB, speed);
if(dir == FORWARD) {
DEBUG("forward...\r\n");
PCA9685_SetLevel(BIN1, 0);
PCA9685_SetLevel(BIN2, 1);
bin1_value = 0;
bin2_value = 1;
} else {
DEBUG("backward...\r\n");
PCA9685_SetLevel(BIN1, 1);
PCA9685_SetLevel(BIN2, 0);
bin1_value = 1;
bin2_value = 0;
}
}
}
/**
* Motor stop rotation.
*
* @param motor: Motor A and Motor B.
*
* Example:
* @code
* Motor_Stop(MOTORA);
*/
void Motor_Stop(UBYTE motor)
{
if(motor == MOTORA) {
PCA9685_SetPwmDutyCycle(PWMA, 0);
} else {
PCA9685_SetPwmDutyCycle(PWMB, 0);
}
}
/**
* Returns motor direction.
* 1 - forward
* 0 - backward
*
* @param motor: Motor A and Motor B
*
* Example:
* @code
* Motor_Direction(MOTORA);
*/
UBYTE Motor_Direction(UBYTE motor)
{
if(motor == MOTORA) {
if(ain1_value == 0 && ain2_value == 1)
return 1;
else if(ain1_value == 1 && ain2_value == 0)
return 0;
}
else {
if(bin1_value == 0 && bin2_value == 1)
return 1;
else if(bin1_value == 1 && bin2_value == 0)
return 0;
}
}
================================================
FILE: lidarbot_base/src/PCA9685.c
================================================
/*****************************************************************************
* | File : PCA9685.c
* | Author : Waveshare team
* | Function : Drive PCA9685
* | Info :
* The PCA9685 is an I2C-bus controlled 16-channel LED
* controller optimized for LCD Red/Green/Blue/Amber
* (RGBA) color backlighting applications.
*----------------
* | This version: V1.0
* | Date : 2018-09-04
* | Info : Basic version
*
******************************************************************************/
#include "lidarbot_base/PCA9685.h"
#include "lidarbot_base/Debug.h" //DEBUG()
#include <math.h> //floor()
#include <stdio.h>
/**
* Write bytes in PCA9685
*
* @param reg: register.
* @param value: value.
*
* Example:
* PCA9685_WriteByte(0x00, 0xff);
*/
static void PCA9685_WriteByte(UBYTE reg, UBYTE value)
{
I2C_Write_Byte(reg, value);
}
/**
* read byte in PCA9685.
*
* @param reg: register.
*
* Example:
* UBYTE buf = PCA9685_ReadByte(0x00);
*/
static UBYTE PCA9685_ReadByte(UBYTE reg)
{
return I2C_Read_Byte(reg);
}
/**
* Set the PWM output.
*
* @param channel: 16 output channels. //(0 ~ 15)
* @param on: 12-bit register will hold avalue for the ON time. //(0 ~ 4095)
* @param off: 12-bit register will hold the value for the OFF time. //(0 ~ 4095)
*
* @For more information, please see page 15 - page 19 of the datasheet.
* Example:
* PCA9685_SetPWM(0, 0, 4095);
*/
static void PCA9685_SetPWM(UBYTE channel, UWORD on, UWORD off)
{
PCA9685_WriteByte(LED0_ON_L + 4*channel, on & 0xFF);
PCA9685_WriteByte(LED0_ON_H + 4*channel, on >> 8);
PCA9685_WriteByte(LED0_OFF_L + 4*channel, off & 0xFF);
PCA9685_WriteByte(LED0_OFF_H + 4*channel, off >> 8);
}
/**
* PCA9685 Initialize.
* For the PCA9685, the device address can be controlled by setting A0-A5.
* On our driver board, control is set by setting A0-A4, and A5 is grounded.
*
* @param addr: PCA9685 address. //0x40 ~ 0x5f
*
* Example:
* PCA9685_Init(0x40);
*/
void PCA9685_Init(char addr)
{
DEV_I2C_Init(addr);
I2C_Write_Byte(MODE1, 0x00);
}
/**
* Set the frequency (PWM_PRESCALE) and restart.
*
* For the PCA9685, Each channel output has its own 12-bit
* resolution (4096 steps) fixed frequency individual PWM
* controller that operates at a programmable frequency
* from a typical of 40 Hz to 1000 Hz with a duty cycle
* that is adjustable from 0 % to 100 %
*
* @param freq: Output frequency. //40 ~ 1000
*
* Example:
* PCA9685_SetPWMFreq(50);
*/
void PCA9685_SetPWMFreq(UWORD freq)
{
freq *= 0.9; // Correct for overshoot in the frequency setting (see issue #11).
double prescaleval = 25000000.0;
prescaleval /= 4096.0;
prescaleval /= freq;
prescaleval -= 1;
DEBUG("prescaleval = %lf\r\n", prescaleval);
UBYTE prescale = floor(prescaleval + 0.5);
DEBUG("prescaleval = %lf\r\n", prescaleval);
UBYTE oldmode = PCA9685_ReadByte(MODE1);
UBYTE newmode = (oldmode & 0x7F) | 0x10; // sleep
PCA9685_WriteByte(MODE1, newmode); // go to sleep
PCA9685_WriteByte(PRESCALE, prescale); // set the prescaler
PCA9685_WriteByte(MODE1, oldmode);
DEV_Delay_ms(5);
PCA9685_WriteByte(MODE1, oldmode | 0x80); // This sets the MODE1 register to turn on auto increment.
}
/**
* Set channel output the PWM duty cycle.
*
* @param channel: 16 output channels. //(0 ~ 15)
* @param pulse: duty cycle. //(0 ~ 100 == 0% ~ 100%)
*
* Example:
* PCA9685_SetPwmDutyCycle(1, 100);
*/
void PCA9685_SetPwmDutyCycle(UBYTE channel, UWORD pulse)
{
PCA9685_SetPWM(channel, 0, pulse * (4096 / 100) - 1);
}
/**
* Set channel output level.
*
* @param channel: 16 output channels. //(0 ~ 15)
* @param value: output level, 0 low level, 1 high level. //0 or 1
*
* Example:
* PCA9685_SetLevel(3, 1);
*/
void PCA9685_SetLevel(UBYTE channel, UWORD value)
{
if (value == 1)
PCA9685_SetPWM(channel, 0, 4095);
else
PCA9685_SetPWM(channel, 0, 0);
}
================================================
FILE: lidarbot_base/src/lidarbot_hardware.cpp
================================================
#include "lidarbot_base/lidarbot_hardware.hpp"
namespace lidarbot_base
{
LidarbotHardware::LidarbotHardware()
: logger_(rclcpp::get_logger("LidarbotHardware"))
{}
CallbackReturn LidarbotHardware::on_init(const hardware_interface::HardwareInfo & info)
{
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
{
return CallbackReturn::ERROR;
}
RCLCPP_INFO(logger_, "Initializing...");
config_.left_wheel_name = info_.hardware_parameters["left_wheel_name"];
config_.right_wheel_name = info_.hardware_parameters["right_wheel_name"];
config_.enc_ticks_per_rev = std::stoi(info_.hardware_parameters["enc_ticks_per_rev"]);
config_.loop_rate = std::stod(info_.hardware_parameters["loop_rate"]);
// Set up wheels
left_wheel_.setup(config_.left_wheel_name, config_.enc_ticks_per_rev);
right_wheel_.setup(config_.right_wheel_name, config_.enc_ticks_per_rev);
RCLCPP_INFO(logger_, "Finished initialization");
return CallbackReturn::SUCCESS;
}
std::vector<hardware_interface::StateInterface> LidarbotHardware::export_state_interfaces()
{
// Set up a position and velocity interface for each wheel
std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.emplace_back(hardware_interface::StateInterface(left_wheel_.name, hardware_interface::HW_IF_VELOCITY, &left_wheel_.velocity));
state_interfaces.emplace_back(hardware_interface::StateInterface(left_wheel_.name, hardware_interface::HW_IF_POSITION, &left_wheel_.position));
state_interfaces.emplace_back(hardware_interface::StateInterface(right_wheel_.name, hardware_interface::HW_IF_VELOCITY, &right_wheel_.velocity));
state_interfaces.emplace_back(hardware_interface::StateInterface(right_wheel_.name, hardware_interface::HW_IF_POSITION, &right_wheel_.position));
return state_interfaces;
}
std::vector<hardware_interface::CommandInterface> LidarbotHardware::export_command_interfaces()
{
// Set up a velocity command for each wheel
std::vector<hardware_interface::CommandInterface> command_interfaces;
command_interfaces.emplace_back(hardware_interface::CommandInterface(left_wheel_.name, hardware_interface::HW_IF_VELOCITY, &left_wheel_.command));
command_interfaces.emplace_back(hardware_interface::CommandInterface(right_wheel_.name, hardware_interface::HW_IF_VELOCITY, &right_wheel_.command));
return command_interfaces;
}
CallbackReturn LidarbotHardware::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
{
RCLCPP_INFO(logger_, "Configuring motors and encoders...");
// Initialize motor driver
Motor_Init();
// Initialize wiringPi using GPIO BCM pin numbers
wiringPiSetupGpio();
// Setup GPIO encoder interrupt and direction pins
pinMode(LEFT_WHL_ENC_INT, INPUT);
pinMode(RIGHT_WHL_ENC_INT, INPUT);
pinMode(LEFT_WHL_ENC_DIR, INPUT);
pinMode(RIGHT_WHL_ENC_DIR, INPUT);
// Setup pull up resistors on encoder interrupt pins
pullUpDnControl(LEFT_WHL_ENC_INT, PUD_UP);
pullUpDnControl(RIGHT_WHL_ENC_INT, PUD_UP);
// Initialize encoder interrupts for falling signal states
wiringPiISR(LEFT_WHL_ENC_INT, INT_EDGE_FALLING, left_wheel_pulse);
wiringPiISR(RIGHT_WHL_ENC_INT, INT_EDGE_FALLING, right_wheel_pulse);
RCLCPP_INFO(logger_, "Successfully configured motors and encoders!");
return CallbackReturn::SUCCESS;
}
CallbackReturn LidarbotHardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
{
RCLCPP_INFO(logger_, "Starting controller ...");
return CallbackReturn::SUCCESS;
}
CallbackReturn LidarbotHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/)
{
RCLCPP_INFO(logger_, "Stopping Controller...");
return CallbackReturn::SUCCESS;
}
return_type LidarbotHardware::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
{
// Obtain elapsed time
double delta_seconds = period.seconds();
// Obtain encoder values
read_encoder_values(&left_wheel_.encoder_ticks, &right_wheel_.encoder_ticks);
// Calculate wheel positions and velocities
double previous_position = left_wheel_.position;
left_wheel_.position = left_wheel_.calculate_encoder_angle();
left_wheel_.velocity = (left_wheel_.position - previous_position) / delta_seconds;
previous_position = right_wheel_.position;
right_wheel_.position = right_wheel_.calculate_encoder_angle();
right_wheel_.velocity = (right_wheel_.position - previous_position) / delta_seconds;
return return_type::OK;
}
return_type LidarbotHardware::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
{
double left_motor_counts_per_loop = left_wheel_.command / left_wheel_.rads_per_tick / config_.loop_rate;
double right_motor_counts_per_loop = right_wheel_.command / right_wheel_.rads_per_tick / config_.loop_rate;
// Send commands to motor driver
set_motor_speeds(left_motor_counts_per_loop, right_motor_counts_per_loop);
return return_type::OK;
}
} // namespace lidarbot_base
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(
lidarbot_base::LidarbotHardware,
hardware_interface::SystemInterface)
================================================
FILE: lidarbot_base/src/motor_checks_client.cpp
================================================
// Client node requests motor checks to be carried out
#include "rclcpp/rclcpp.hpp"
#include "std_srvs/srv/trigger.hpp"
// C++ namespace for representing time durations
using namespace std::chrono_literals;
int main(int argc, char **argv) {
// Initialize the rclcpp library
rclcpp::init(argc, argv);
// Create a shared pointer to a Node type and name it "motor_checks_client"
std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("motor_checks_client");
// Create a client inside the node to call the "checks" server node
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr client =
node->create_client<std_srvs::srv::Trigger>("checks");
// Create the request, which is empty
auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
// Wait 2 seconds for the service to be activated
while (!client->wait_for_service(2s)) {
// if ROS is shutdown before the service is activated, show this error
if (!rclcpp::ok()) {
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
return 0;
}
// Print in the screen some information so the user knows what is happening
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");
}
// Client sends its asynchronous request
auto result = client->async_send_request(request);
// Wait for the result
if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
// Get the response's success field to see if all checks passed
if (result.get()->success) {
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "The checks were successful!");
} else {
// TODO: Message below does get outputted. Conversion seems to be the culprit
RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "The checks were not successful: %s", result.get()->message.c_str());
}
} else {
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service 'checks'");
}
// Shut down rclcpp and client node
rclcpp::shutdown();
return 0;
}
================================================
FILE: lidarbot_base/src/motor_checks_server.cpp
================================================
// Server node runs motor tests to confirm that the motor is working correctly by moving both motors forward
#include "rclcpp/rclcpp.hpp"
#include "std_srvs/srv/trigger.hpp"
#include "lidarbot_base/motor_encoder.h"
// Reset pulse counters
void reset_pulse_counters()
{
right_wheel_pulse_count = 0;
left_wheel_pulse_count = 0;
}
// Move a specified motor forward
bool move_motor(int motor_id)
{
// Motor_id
// MOTORA - 0 (left motor)
// MOTORB - 1 (right motor)
reset_pulse_counters();
sleep(2);
// Move motor FORWARD for 2 seconds at 50% speed
Motor_Run(motor_id, FORWARD, 50);
sleep(2);
Motor_Stop(motor_id);
// Motor counts should be greater than 0 to confirm that the motor moved forward
if (motor_id == MOTORA) {
if (left_wheel_pulse_count> 0) {
return true;
} else return false;
}
if (motor_id == MOTORB) {
if (right_wheel_pulse_count > 0) {
return true;
} return false;
}
}
// DDS helps pass the request and response between client and server
void checkMotors(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
std::shared_ptr<std_srvs::srv::Trigger::Response> response) {
// Prepare response
response->success = true;
response->message = "";
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Received request to check motors...");
// Left motor check
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Checking left motor...");
auto left_motor_passed = move_motor(MOTORA);
if (!left_motor_passed) {
response->success = false;
response->message += "Left motor check failed, confirm motor wiring.";
}
// Right motor check
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Checking right motor...");
auto right_motor_passed = move_motor(MOTORB);
if (!right_motor_passed) {
response->success = false;
response->message += "Right motor check failed, confirm motor wiring.";
}
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sending back response...");
}
int main(int argc, char **argv)
{
// Initialize motor driver
Motor_Init();
// Initialize wiringPi using GPIO BCM pin numbers
wiringPiSetupGpio();
// Setup GPIO encoder interrupt and direction pins
pinMode(LEFT_WHL_ENC_INT, INPUT);
pinMode(RIGHT_WHL_ENC_INT, INPUT);
pinMode(LEFT_WHL_ENC_DIR, INPUT);
pinMode(RIGHT_WHL_ENC_DIR, INPUT);
// Setup pull up resistors on encoder pins
pullUpDnControl(LEFT_WHL_ENC_INT, PUD_UP);
pullUpDnControl(RIGHT_WHL_ENC_INT, PUD_UP);
// Initialize encoder interrupts for falling signal states
wiringPiISR(LEFT_WHL_ENC_INT, INT_EDGE_FALLING, left_wheel_pulse);
wiringPiISR(RIGHT_WHL_ENC_INT, INT_EDGE_FALLING, right_wheel_pulse);
// Initialize the rclcpp library
rclcpp::init(argc, argv);
// Create a shared pointer to a Node type and name it "motor_checks_server"
std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("motor_checks_server");
// Create a "checks" service with a checkMotors callback
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr service =
node->create_service<std_srvs::srv::Trigger>("checks", &checkMotors);
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to check motors");
// Spin the node until it's terminated
rclcpp::spin(node);
rclcpp::shutdown();
}
================================================
FILE: lidarbot_base/src/motor_encoder.c
================================================
// This file calculate encoder ticks and sets the speed each motor should run at according to
// the commands received from the Waveshare Motor Driver system hardware component
#include "lidarbot_base/motor_encoder.h"
#include <math.h>
// Initialize pulse counters
int left_wheel_pulse_count = 0;
int right_wheel_pulse_count = 0;
// Initialize wheel directions
// 1 - forward, 0 - backward
int left_wheel_direction = 1;
int right_wheel_direction = 1;
// Read wheel encoder values
void read_encoder_values(int *left_encoder_value, int *right_encoder_value) {
*left_encoder_value = left_wheel_pulse_count;
*right_encoder_value = right_wheel_pulse_count;
}
// Left wheel callback function
void left_wheel_pulse() {
// left wheel direction
// 1 - forward
// 0 - backward
// Read encoder direction value for left wheel
left_wheel_direction = digitalRead(LEFT_WHL_ENC_DIR);
if (left_wheel_direction == 1)
left_wheel_pulse_count++;
else
left_wheel_pulse_count--;
}
// Right wheel callback function
void right_wheel_pulse() {
// right wheel direction
// 1 - forward,
// 0 - backward
// Read encoder direction value for right wheel
right_wheel_direction = digitalRead(RIGHT_WHL_ENC_DIR);
if (right_wheel_direction == 1)
right_wheel_pulse_count++;
else
right_wheel_pulse_count--;
}
// Set each motor speed from the respective velocity command interface
void set_motor_speeds(double left_wheel_command, double right_wheel_command) {
// Initialize DIR enum variables
DIR left_motor_direction;
DIR right_motor_direction;
// Tune motor speeds by adjusting the command coefficients. These are
// dependent on the number of encoder ticks. 3000 ticks and above work well
// with coefficients of 1.0
double left_motor_speed = ceil(left_wheel_command * 1.65);
double right_motor_speed = ceil(right_wheel_command * 1.65);
// Set motor directions
if (left_motor_speed > 0)
left_motor_direction = FORWARD;
else
left_motor_direction = BACKWARD;
if (right_motor_speed > 0)
right_motor_direction = FORWARD;
else
right_motor_direction = BACKWARD;
// Run motors with specified direction and speeds
Motor_Run(MOTORA, left_motor_direction, (int)abs(left_motor_speed));
Motor_Run(MOTORB, right_motor_direction, (int)abs(right_motor_speed));
}
void handler(int signo) {
Motor_Stop(MOTORA);
Motor_Stop(MOTORB);
exit(0);
}
================================================
FILE: lidarbot_base/src/wheel.cpp
================================================
#include "lidarbot_base/wheel.hpp"
Wheel::Wheel(const std::string &wheel_name, int ticks_per_rev)
{
setup(wheel_name, ticks_per_rev);
}
void Wheel::setup(const std::string &wheel_name, int ticks_per_rev)
{
name = wheel_name;
rads_per_tick = (2*M_PI)/ticks_per_rev;
}
double Wheel::calculate_encoder_angle()
{
return encoder_ticks * rads_per_tick;
}
================================================
FILE: lidarbot_bringup/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 3.8)
project(lidarbot_bringup)
set(THIS_PACKAGE_INCLUDE_DEPENDS
hardware_interface
pluginlib
rclcpp
rclcpp_lifecycle
)
find_package(ament_cmake REQUIRED)
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
find_package(${Dependency} REQUIRED)
endforeach()
# COMPILE
add_library(mpu6050_hardware_interface SHARED
src/mpu6050_hardware_interface.cpp
src/mpu6050_lib.cpp
)
target_include_directories(mpu6050_hardware_interface PRIVATE include)
ament_target_dependencies(mpu6050_hardware_interface ${THIS_PACKAGE_INCLUDE_DEPENDS})
# Link i2c library to mpu6050_hardware_interface target
target_link_libraries(mpu6050_hardware_interface i2c)
# Export hardware plugin
pluginlib_export_plugin_description_file(hardware_interface mpu6050_hardware.xml)
# INSTALL
install(
DIRECTORY include/
DESTINATION include/mpu6050_hardware_interface
)
install(
TARGETS mpu6050_hardware_interface
DESTINATION share/${PROJECT_NAME}
RUNTIME DESTINATION lib
LIBRARY DESTINATION lib/${PROJECT_NAME}
)
install(
DIRECTORY config launch
DESTINATION share/${PROJECT_NAME}
)
# EXPORTS
ament_export_libraries(mpu6050_hardware_interface)
include_directories(include)
# Create Cpp executables
add_executable(mpu6050_offsets src/mpu6050_lib.cpp src/mpu6050_offsets.cpp)
add_executable(mpu6050_covariances src/mpu6050_lib.cpp src/mpu6050_covariances.cpp)
# Install Cpp executables
install(TARGETS
mpu6050_offsets
mpu6050_covariances
DESTINATION lib/${PROJECT_NAME}
)
# Link i2c to mpu6050_offsets and mpu6050_covariances targets
target_link_libraries(mpu6050_offsets i2c)
target_link_libraries(mpu6050_covariances i2c)
ament_package()
================================================
FILE: lidarbot_bringup/LICENSE
================================================
Attribution-ShareAlike 4.0 International
=======================================================================
Creative Commons Corporation ("Creative Commons") is not a law firm and
does not provide legal services or legal advice. Distribution of
Creative Commons public licenses does not create a lawyer-client or
other relationship. Creative Commons makes its licenses and related
information available on an "as-is" basis. Creative Commons gives no
warranties regarding its licenses, any material licensed under their
terms and conditions, or any related information. Creative Commons
disclaims all liability for damages resulting from their use to the
fullest extent possible.
Using Creative Commons Public Licenses
Creative Commons public licenses provide a standard set of terms and
conditions that creators and other rights holders may use to share
original works of authorship and other material subject to copyright
and certain other rights specified in the public license below. The
following considerations are for informational purposes only, are not
exhaustive, and do not form part of our licenses.
Considerations for licensors: Our public licenses are
intended for use by those authorized to give the public
permission to use material in ways otherwise restricted by
copyright and certain other rights. Our licenses are
irrevocable. Licensors should read and understand the terms
and conditions of the license they choose before applying it.
Licensors should also secure all rights necessary before
applying our licenses so that the public can reuse the
material as expected. Licensors should clearly mark any
material not subject to the license. This includes other CC-
licensed material, or material used under an exception or
limitation to copyright. More considerations for licensors:
wiki.creativecommons.org/Considerations_for_licensors
Considerations for the public: By using one of our public
licenses, a licensor grants the public permission to use the
licensed material under specified terms and conditions. If
the licensor's permission is not necessary for any reason--for
example, because of any applicable exception or limitation to
copyright--then that use is not regulated by the license. Our
licenses grant only permissions under copyright and certain
other rights that a licensor has authority to grant. Use of
the licensed material may still be restricted for other
reasons, including because others have copyright or other
rights in the material. A licensor may make special requests,
such as asking that all changes be marked or described.
Although not required by our licenses, you are encouraged to
respect those requests where reasonable. More_considerations
for the public:
wiki.creativecommons.org/Considerations_for_licensees
=======================================================================
Creative Commons Attribution-ShareAlike 4.0 International Public
License
By exercising the Licensed Rights (defined below), You accept and agree
to be bound by the terms and conditions of this Creative Commons
Attribution-ShareAlike 4.0 International Public License ("Public
License"). To the extent this Public License may be interpreted as a
contract, You are granted the Licensed Rights in consideration of Your
acceptance of these terms and conditions, and the Licensor grants You
such rights in consideration of benefits the Licensor receives from
making the Licensed Material available under these terms and
conditions.
Section 1 -- Definitions.
a. Adapted Material means material subject to Copyright and Similar
Rights that is derived from or based upon the Licensed Material
and in which the Licensed Material is translated, altered,
arranged, transformed, or otherwise modified in a manner requiring
permission under the Copyright and Similar Rights held by the
Licensor. For purposes of this Public License, where the Licensed
Material is a musical work, performance, or sound recording,
Adapted Material is always produced where the Licensed Material is
synched in timed relation with a moving image.
b. Adapter's License means the license You apply to Your Copyright
and Similar Rights in Your contributions to Adapted Material in
accordance with the terms and conditions of this Public License.
c. BY-SA Compatible License means a license listed at
creativecommons.org/compatiblelicenses, approved by Creative
Commons as essentially the equivalent of this Public License.
d. Copyright and Similar Rights means copyright and/or similar rights
closely related to copyright including, without limitation,
performance, broadcast, sound recording, and Sui Generis Database
Rights, without regard to how the rights are labeled or
categorized. For purposes of this Public License, the rights
specified in Section 2(b)(1)-(2) are not Copyright and Similar
Rights.
e. Effective Technological Measures means those measures that, in the
absence of proper authority, may not be circumvented under laws
fulfilling obligations under Article 11 of the WIPO Copyright
Treaty adopted on December 20, 1996, and/or similar international
agreements.
f. Exceptions and Limitations means fair use, fair dealing, and/or
any other exception or limitation to Copyright and Similar Rights
that applies to Your use of the Licensed Material.
g. License Elements means the license attributes listed in the name
of a Creative Commons Public License. The License Elements of this
Public License are Attribution and ShareAlike.
h. Licensed Material means the artistic or literary work, database,
or other material to which the Licensor applied this Public
License.
i. Licensed Rights means the rights granted to You subject to the
terms and conditions of this Public License, which are limited to
all Copyright and Similar Rights that apply to Your use of the
Licensed Material and that the Licensor has authority to license.
j. Licensor means the individual(s) or entity(ies) granting rights
under this Public License.
k. Share means to provide material to the public by any means or
process that requires permission under the Licensed Rights, such
as reproduction, public display, public performance, distribution,
dissemination, communication, or importation, and to make material
available to the public including in ways that members of the
public may access the material from a place and at a time
individually chosen by them.
l. Sui Generis Database Rights means rights other than copyright
resulting from Directive 96/9/EC of the European Parliament and of
the Council of 11 March 1996 on the legal protection of databases,
as amended and/or succeeded, as well as other essentially
equivalent rights anywhere in the world.
m. You means the individual or entity exercising the Licensed Rights
under this Public License. Your has a corresponding meaning.
Section 2 -- Scope.
a. License grant.
1. Subject to the terms and conditions of this Public License,
the Licensor hereby grants You a worldwide, royalty-free,
non-sublicensable, non-exclusive, irrevocable license to
exercise the Licensed Rights in the Licensed Material to:
a. reproduce and Share the Licensed Material, in whole or
in part; and
b. produce, reproduce, and Share Adapted Material.
2. Exceptions and Limitations. For the avoidance of doubt, where
Exceptions and Limitations apply to Your use, this Public
License does not apply, and You do not need to comply with
its terms and conditions.
3. Term. The term of this Public License is specified in Section
6(a).
4. Media and formats; technical modifications allowed. The
Licensor authorizes You to exercise the Licensed Rights in
all media and formats whether now known or hereafter created,
and to make technical modifications necessary to do so. The
Licensor waives and/or agrees not to assert any right or
authority to forbid You from making technical modifications
necessary to exercise the Licensed Rights, including
technical modifications necessary to circumvent Effective
Technological Measures. For purposes of this Public License,
simply making modifications authorized by this Section 2(a)
(4) never produces Adapted Material.
5. Downstream recipients.
a. Offer from the Licensor -- Licensed Material. Every
recipient of the Licensed Material automatically
receives an offer from the Licensor to exercise the
Licensed Rights under the terms and conditions of this
Public License.
b. Additional offer from the Licensor -- Adapted Material.
Every recipient of Adapted Material from You
automatically receives an offer from the Licensor to
exercise the Licensed Rights in the Adapted Material
under the conditions of the Adapter's License You apply.
c. No downstream restrictions. You may not offer or impose
any additional or different terms or conditions on, or
apply any Effective Technological Measures to, the
Licensed Material if doing so restricts exercise of the
Licensed Rights by any recipient of the Licensed
Material.
6. No endorsement. Nothing in this Public License constitutes or
may be construed as permission to assert or imply that You
are, or that Your use of the Licensed Material is, connected
with, or sponsored, endorsed, or granted official status by,
the Licensor or others designated to receive attribution as
provided in Section 3(a)(1)(A)(i).
b. Other rights.
1. Moral rights, such as the right of integrity, are not
licensed under this Public License, nor are publicity,
privacy, and/or other similar personality rights; however, to
the extent possible, the Licensor waives and/or agrees not to
assert any such rights held by the Licensor to the limited
extent necessary to allow You to exercise the Licensed
Rights, but not otherwise.
2. Patent and trademark rights are not licensed under this
Public License.
3. To the extent possible, the Licensor waives any right to
collect royalties from You for the exercise of the Licensed
Rights, whether directly or through a collecting society
under any voluntary or waivable statutory or compulsory
licensing scheme. In all other cases the Licensor expressly
reserves any right to collect such royalties.
Section 3 -- License Conditions.
Your exercise of the Licensed Rights is expressly made subject to the
following conditions.
a. Attribution.
1. If You Share the Licensed Material (including in modified
form), You must:
a. retain the following if it is supplied by the Licensor
with the Licensed Material:
i. identification of the creator(s) of the Licensed
Material and any others designated to receive
attribution, in any reasonable manner requested by
the Licensor (including by pseudonym if
designated);
ii. a copyright notice;
iii. a notice that refers to this Public License;
iv. a notice that refers to the disclaimer of
warranties;
v. a URI or hyperlink to the Licensed Material to the
extent reasonably practicable;
b. indicate if You modified the Licensed Material and
retain an indication of any previous modifications; and
c. indicate the Licensed Material is licensed under this
Public License, and include the text of, or the URI or
hyperlink to, this Public License.
2. You may satisfy the conditions in Section 3(a)(1) in any
reasonable manner based on the medium, means, and context in
which You Share the Licensed Material. For example, it may be
reasonable to satisfy the conditions by providing a URI or
hyperlink to a resource that includes the required
information.
3. If requested by the Licensor, You must remove any of the
information required by Section 3(a)(1)(A) to the extent
reasonably practicable.
b. ShareAlike.
In addition to the conditions in Section 3(a), if You Share
Adapted Material You produce, the following conditions also apply.
1. The Adapter's License You apply must be a Creative Commons
license with the same License Elements, this version or
later, or a BY-SA Compatible License.
2. You must include the text of, or the URI or hyperlink to, the
Adapter's License You apply. You may satisfy this condition
in any reasonable manner based on the medium, means, and
context in which You Share Adapted Material.
3. You may not offer or impose any additional or different terms
or conditions on, or apply any Effective Technological
Measures to, Adapted Material that restrict exercise of the
rights granted under the Adapter's License You apply.
Section 4 -- Sui Generis Database Rights.
Where the Licensed Rights include Sui Generis Database Rights that
apply to Your use of the Licensed Material:
a. for the avoidance of doubt, Section 2(a)(1) grants You the right
to extract, reuse, reproduce, and Share all or a substantial
portion of the contents of the database;
b. if You include all or a substantial portion of the database
contents in a database in which You have Sui Generis Database
Rights, then the database in which You have Sui Generis Database
Rights (but not its individual contents) is Adapted Material,
including for purposes of Section 3(b); and
c. You must comply with the conditions in Section 3(a) if You Share
all or a substantial portion of the contents of the database.
For the avoidance of doubt, this Section 4 supplements and does not
replace Your obligations under this Public License where the Licensed
Rights include other Copyright and Similar Rights.
Section 5 -- Disclaimer of Warranties and Limitation of Liability.
a. UNLESS OTHERWISE SEPARATELY UNDERTAKEN BY THE LICENSOR, TO THE
EXTENT POSSIBLE, THE LICENSOR OFFERS THE LICENSED MATERIAL AS-IS
AND AS-AVAILABLE, AND MAKES NO REPRESENTATIONS OR WARRANTIES OF
ANY KIND CONCERNING THE LICENSED MATERIAL, WHETHER EXPRESS,
IMPLIED, STATUTORY, OR OTHER. THIS INCLUDES, WITHOUT LIMITATION,
WARRANTIES OF TITLE, MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE, NON-INFRINGEMENT, ABSENCE OF LATENT OR OTHER DEFECTS,
ACCURACY, OR THE PRESENCE OR ABSENCE OF ERRORS, WHETHER OR NOT
KNOWN OR DISCOVERABLE. WHERE DISCLAIMERS OF WARRANTIES ARE NOT
ALLOWED IN FULL OR IN PART, THIS DISCLAIMER MAY NOT APPLY TO YOU.
b. TO THE EXTENT POSSIBLE, IN NO EVENT WILL THE LICENSOR BE LIABLE
TO YOU ON ANY LEGAL THEORY (INCLUDING, WITHOUT LIMITATION,
NEGLIGENCE) OR OTHERWISE FOR ANY DIRECT, SPECIAL, INDIRECT,
INCIDENTAL, CONSEQUENTIAL, PUNITIVE, EXEMPLARY, OR OTHER LOSSES,
COSTS, EXPENSES, OR DAMAGES ARISING OUT OF THIS PUBLIC LICENSE OR
USE OF THE LICENSED MATERIAL, EVEN IF THE LICENSOR HAS BEEN
ADVISED OF THE POSSIBILITY OF SUCH LOSSES, COSTS, EXPENSES, OR
DAMAGES. WHERE A LIMITATION OF LIABILITY IS NOT ALLOWED IN FULL OR
IN PART, THIS LIMITATION MAY NOT APPLY TO YOU.
c. The disclaimer of warranties and limitation of liability provided
above shall be interpreted in a manner that, to the extent
possible, most closely approximates an absolute disclaimer and
waiver of all liability.
Section 6 -- Term and Termination.
a. This Public License applies for the term of the Copyright and
Similar Rights licensed here. However, if You fail to comply with
this Public License, then Your rights under this Public License
terminate automatically.
b. Where Your right to use the Licensed Material has terminated under
Section 6(a), it reinstates:
1. automatically as of the date the violation is cured, provided
it is cured within 30 days of Your discovery of the
violation; or
2. upon express reinstatement by the Licensor.
For the avoidance of doubt, this Section 6(b) does not affect any
right the Licensor may have to seek remedies for Your violations
of this Public License.
c. For the avoidance of doubt, the Licensor may also offer the
Licensed Material under separate terms or conditions or stop
distributing the Licensed Material at any time; however, doing so
will not terminate this Public License.
d. Sections 1, 5, 6, 7, and 8 survive termination of this Public
License.
Section 7 -- Other Terms and Conditions.
a. The Licensor shall not be bound by any additional or different
terms or conditions communicated by You unless expressly agreed.
b. Any arrangements, understandings, or agreements regarding the
Licensed Material not stated herein are separate from and
independent of the terms and conditions of this Public License.
Section 8 -- Interpretation.
a. For the avoidance of doubt, this Public License does not, and
shall not be interpreted to, reduce, limit, restrict, or impose
conditions on any use of the Licensed Material that could lawfully
be made without permission under this Public License.
b. To the extent possible, if any provision of this Public License is
deemed unenforceable, it shall be automatically reformed to the
minimum extent necessary to make it enforceable. If the provision
cannot be reformed, it shall be severed from this Public License
without affecting the enforceability of the remaining terms and
conditions.
c. No term or condition of this Public License will be waived and no
failure to comply consented to unless expressly agreed to by the
Licensor.
d. Nothing in this Public License constitutes or may be interpreted
as a limitation upon, or waiver of, any privileges and immunities
that apply to the Licensor or You, including from the legal
processes of any jurisdiction or authority.
=======================================================================
Creative Commons is not a party to its public
licenses. Notwithstanding, Creative Commons may elect to apply one of
its public licenses to material it publishes and in those instances
will be considered the �Licensor.� The text of the Creative Commons
public licenses is dedicated to the public domain under the CC0 Public
Domain Dedication. Except for the limited purpose of indicating that
material is shared under a Creative Commons public license or as
otherwise permitted by the Creative Commons policies published at
creativecommons.org/policies, Creative Commons does not authorize the
use of the trademark "Creative Commons" or any other trademark or logo
of Creative Commons without its prior written consent including,
without limitation, in connection with any unauthorized modifications
to any of its public licenses or any other arrangements,
understandings, or agreements concerning use of licensed material. For
the avoidance of doubt, this paragraph does not form part of the
public licenses.
Creative Commons may be contacted at creativecommons.org.
================================================
FILE: lidarbot_bringup/config/controllers.yaml
================================================
controller_manager: # Node name
ros__parameters:
update_rate: 30 # Has to be an integer otherwise errors are encountered
diff_controller:
type: diff_drive_controller/DiffDriveController
joint_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
imu_broadcaster:
type: imu_sensor_broadcaster/IMUSensorBroadcaster
diff_controller:
ros__parameters:
publish_rate: 50.0 # Has to be a double otherwise errors are encountered
base_frame_id: base_footprint
left_wheel_names: ['left_wheel_joint']
right_wheel_names: ['right_wheel_joint']
wheel_separation: 0.134
wheel_radius: 0.0335
# Obtained from https://wiki.ros.org/diff_drive_controller
pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
# Whether the input has a time stamp or not
use_stamped_vel: false
# Odometry fused with IMU is published by robot_localization, so no need to publish a TF
# based on encoders alone (https://wiki.ros.org/diff_drive_controller)
enable_odom_tf: false
imu_broadcaster:
ros__parameters:
sensor_name: mpu6050
frame_id: imu_link
# 500 data points used to calculate covariances
static_covariance_orientation: [2.63882e-06, 0.0, 0.0, 0.0, 7.50018e-06, 0.0, 0.0, 0.0, 2.89257e-09]
static_covariance_angular_velocity: [2.71413e-07, 0.0, 0.0, 0.0, 6.79488e-07, 0.0, 0.0, 0.0, 4.37879e-07]
static_covariance_linear_acceleration: [0.00133755, 0.0, 0.0, 0.0, 0.000209753, 0.0, 0.0, 0.0, 0.00143276]
================================================
FILE: lidarbot_bringup/config/gazebo_ros2_ctl_use_sim.yaml
================================================
controller_manager: # Node name
ros__parameters:
use_sim_time: true
================================================
FILE: lidarbot_bringup/include/lidarbot_bringup/mpu6050_hardware_interface.hpp
================================================
#ifndef _LIDARBOT_BRINGUP__MPU6050_HARDWARE_HPP_
#define _LIDARBOT_BRINGUP__MPU6050_HARDWARE_HPP_
#include <memory>
#include <thread>
#include <vector>
#include <string>
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/sensor_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "mpu6050_lib.h"
using hardware_interface::return_type;
using hardware_interface::CallbackReturn;
namespace lidarbot_bringup
{
class MPU6050Hardware : public hardware_interface::SensorInterface
{
public:
MPU6050Hardware();
CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
private:
Quaternion orientation;
double angular_vel_x;
double angular_vel_y;
double angular_vel_z;
double linear_accel_x;
double linear_accel_y;
double linear_accel_z;
Quaternion quat;
float euler_angles[3];
float gyro_values[3];
float accel_values[3];
rclcpp::Logger logger_;
};
} // namespace lidarbot_bringup
#endif
================================================
FILE: lidarbot_bringup/include/lidarbot_bringup/mpu6050_lib.h
================================================
//-------------------------------MPU6050 Accelerometer and Gyroscope C++ library-----------------------------
//Made changes to class and renamed file. Adapted from Alex Mous (https://github.com/alex-mous/MPU6050-C-CPP-Library-for-Raspberry-Pi)
//Copyright (c) 2019, Alex Mous
//Licensed under the CC BY-NC SA 4.0
//-----------------------MODIFY THESE PARAMETERS-----------------------
#define GYRO_RANGE 0 //Select which gyroscope range to use (see the table below) - Default is 0
// Gyroscope Range
// 0 +/- 250 degrees/second
// 1 +/- 500 degrees/second
// 2 +/- 1000 degrees/second
// 3 +/- 2000 degrees/second
//See the MPU6000 Register Map for more information
#define ACCEL_RANGE 0 //Select which accelerometer range to use (see the table below) - Default is 0
// Accelerometer Range
// 0 +/- 2g
// 1 +/- 4g
// 2 +/- 8g
// 3 +/- 16g
//See the MPU6000 Register Map for more information
//Offsets - supply your own here (calculate offsets with getOffsets function)
// Gyroscope
#define G_OFF_X -187
#define G_OFF_Y 757
#define G_OFF_Z -157
// Accelerometer
#define A_OFF_X -13450
#define A_OFF_Y 2803
#define A_OFF_Z -14419
//-----------------------END MODIFY THESE PARAMETERS-----------------------
#include <iostream>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <time.h>
extern "C" {
#include <linux/i2c-dev.h>
#include <i2c/smbus.h>
}
#include <cmath>
#include <thread>
#define _POSIX_C_SOURCE 200809L //Used for calculating time
#define TAU 0.05 //Complementary filter percentage
#define RAD_T_DEG 57.29577951308 //Radians to degrees (180/PI)
#define M_PER_SECS_SQUARED 9.80665 //Acceleration G-unit to m/s^2
//Select the appropriate settings
#if GYRO_RANGE == 1
#define GYRO_SENS 65.5
#define GYRO_CONFIG 0b00001000
#elif GYRO_RANGE == 2
#define GYRO_SENS 32.8
#define GYRO_CONFIG 0b00010000
#elif GYRO_RANGE == 3
#define GYRO_SENS 16.4
#define GYRO_CONFIG 0b00011000
#else //Otherwise, default to 0
#define GYRO_SENS 131.0
#define GYRO_CONFIG 0b00000000
#endif
#undef GYRO_RANGE
#if ACCEL_RANGE == 1
#define ACCEL_SENS 8192.0
#define ACCEL_CONFIG 0b00001000
#elif ACCEL_RANGE == 2
#define ACCEL_SENS 4096.0
#define ACCEL_CONFIG 0b00010000
#elif ACCEL_RANGE == 3
#define ACCEL_SENS 2048.0
#define ACCEL_CONFIG 0b00011000
#else //Otherwise, default to 0
#define ACCEL_SENS 16384.0
#define ACCEL_CONFIG 0b00000000
#endif
#undef ACCEL_RANGE
struct Quaternion
{
double w, x, y, z;
};
class MPU6050 {
private:
void _update();
float _accel_angle[3];
float _gyro_angle[3];
float _angle[3]; //Store all angles (accel roll, accel pitch, accel yaw, gyro roll, gyro pitch, gyro yaw, comb roll, comb pitch comb yaw)
float ax, ay, az, gr, gp, gy; //Temporary storage variables used in _update()
int MPU6050_addr;
int f_dev; //Device file
float dt; //Loop time (recalculated with each loop)
struct timespec start,end; //Create a time structure
bool _first_run = 1; //Variable for whether to set gyro angle to acceleration angle in compFilter
public:
MPU6050(int8_t addr);
MPU6050(int8_t addr, bool run_update_thread);
void getAccelRaw(float *x, float *y, float *z);
void getGyroRaw(float *roll, float *pitch, float *yaw);
void getAccel(float *x, float *y, float *z);
void getGyro(float *roll, float *pitch, float *yaw);
void getOffsets(float *ax_off, float *ay_off, float *az_off, float *gr_off, float *gp_off, float *gy_off);
Quaternion getQuat(float *roll, float *pitch, float *yaw);
int getAngle(int axis, float *result);
bool calc_yaw;
};
================================================
FILE: lidarbot_bringup/launch/camera_launch.py
================================================
# This launch file runs the v4l2_camera node used to start up the raspberry pi camera v1.3
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='v4l2_camera',
executable='v4l2_camera_node',
output='screen',
namespace='camera',
parameters=[{
'image_size': [640, 480],
'time_per_frame': [1, 6],
'camera_frame_id': 'camera_link_optical'
}]
)
])
================================================
FILE: lidarbot_bringup/launch/lidarbot_bringup_launch.py
================================================
# This launch file brings up the physical lidarbot, raspberry pi camera v1.3,
# RPLIDAR A1 and also integrates ros2_control, twist_mux, robot_localization
# and joystick control
# File adapted from https://automaticaddison.com
import os
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
TimerAction,
RegisterEventHandler,
)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, Command
from launch.event_handlers import OnProcessStart
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# Set the path to different files and folders
pkg_path = FindPackageShare(package="lidarbot_bringup").find("lidarbot_bringup")
pkg_description = FindPackageShare(package="lidarbot_description").find(
"lidarbot_description"
)
pkg_teleop = FindPackageShare(package="lidarbot_teleop").find("lidarbot_teleop")
pkg_navigation = FindPackageShare(package="lidarbot_navigation").find(
"lidarbot_navigation"
)
controller_params_file = os.path.join(pkg_path, "config/controllers.yaml")
twist_mux_params_file = os.path.join(pkg_teleop, "config/twist_mux.yaml")
ekf_params_file = os.path.join(pkg_navigation, "config/ekf.yaml")
# Launch configuration variables
use_sim_time = LaunchConfiguration("use_sim_time")
use_ros2_control = LaunchConfiguration("use_ros2_control")
use_robot_localization = LaunchConfiguration("use_robot_localization")
# Declare the launch arguments
declare_use_sim_time_cmd = DeclareLaunchArgument(
name="use_sim_time",
default_value="False",
description="Use simulation (Gazebo) clock if true",
)
declare_use_ros2_control_cmd = DeclareLaunchArgument(
name="use_ros2_control",
default_value="True",
description="Use ros2_control if true",
)
declare_use_robot_localization_cmd = DeclareLaunchArgument(
name="use_robot_localization",
default_value="True",
description="Use robot_localization package if true",
)
# Start robot state publisher
start_robot_state_publisher_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[os.path.join(pkg_description, "launch", "robot_state_publisher_launch.py")]
),
launch_arguments={
"use_sim_time": use_sim_time,
"use_ros2_control": use_ros2_control,
}.items(),
)
robot_description = Command(
["ros2 param get --hide-type /robot_state_publisher robot_description"]
)
# Launch controller manager
start_controller_manager_cmd = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[{"robot_description": robot_description}, controller_params_file],
)
# Spawn diff_controller
start_diff_controller_cmd = Node(
package="controller_manager",
executable="spawner",
arguments=["diff_controller", "--controller-manager", "/controller_manager"],
)
# Spawn joint_state_broadcaser
start_joint_broadcaster_cmd = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_broadcaster", "--controller-manager", "/controller_manager"],
)
# Spawn imu_sensor_broadcaster
start_imu_broadcaster_cmd = Node(
package="controller_manager",
executable="spawner",
arguments=["imu_broadcaster", "--controller-manager", "/controller_manager"],
)
# Delayed controller manager action
start_delayed_controller_manager = TimerAction(
period=2.0, actions=[start_controller_manager_cmd]
)
# Delayed diff_drive_spawner action
start_delayed_diff_drive_spawner = RegisterEventHandler(
event_handler=OnProcessStart(
target_action=start_controller_manager_cmd,
on_start=[start_diff_controller_cmd],
)
)
# Delayed joint_broadcaster_spawner action
start_delayed_joint_broadcaster_spawner = RegisterEventHandler(
event_handler=OnProcessStart(
target_action=start_controller_manager_cmd,
on_start=[start_joint_broadcaster_cmd],
)
)
# Delayed imu_broadcaster_spawner action
start_delayed_imu_broadcaster_spawner = RegisterEventHandler(
event_handler=OnProcessStart(
target_action=start_controller_manager_cmd,
on_start=[start_imu_broadcaster_cmd],
)
)
# Start robot localization using an Extended Kalman Filter
start_robot_localization_cmd = Node(
condition=IfCondition(use_robot_localization),
package="robot_localization",
executable="ekf_node",
parameters=[
ekf_params_file,
{'use_sim_time': LaunchConfiguration('use_sim_time')},
],
remappings=[("/odometry/filtered", "/odom")],
)
# Start joystick node
start_joystick_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[os.path.join(pkg_teleop, "launch", "joystick_launch.py")]
),
launch_arguments={
"use_sim_time": use_sim_time,
}.items(),
)
# Start rplidar node
start_rplidar_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[os.path.join(pkg_path, "launch", "rplidar_launch.py")]
)
)
# Start camera node
start_camera_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[os.path.join(pkg_path, "launch", "camera_launch.py")]
)
)
# Start twist mux
start_twist_mux_cmd = Node(
package="twist_mux",
executable="twist_mux",
parameters=[twist_mux_params_file],
remappings=[("/cmd_vel_out", "/diff_controller/cmd_vel_unstamped")],
)
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_use_ros2_control_cmd)
ld.add_action(declare_use_robot_localization_cmd)
# Add any actions
ld.add_action(start_robot_state_publisher_cmd)
ld.add_action(start_delayed_controller_manager)
ld.add_action(start_delayed_diff_drive_spawner)
ld.add_action(start_delayed_joint_broadcaster_spawner)
ld.add_action(start_delayed_imu_broadcaster_spawner)
ld.add_action(start_robot_localization_cmd)
ld.add_action(start_joystick_cmd)
ld.add_action(start_rplidar_cmd)
ld.add_action(start_camera_cmd)
ld.add_action(start_twist_mux_cmd)
return ld
================================================
FILE: lidarbot_bringup/launch/rplidar_launch.py
================================================
# This launch file runs the rplidar_ros node with the configurations specified
#!/usr/bin/env python3
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
channel_type = LaunchConfiguration('channel_type', default='serial')
serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0')
serial_baudrate = LaunchConfiguration('serial_baudrate', default='115200')
frame_id = LaunchConfiguration('frame_id', default='lidar_link')
inverted = LaunchConfiguration('inverted', default='false')
angle_compensate = LaunchConfiguration('angle_compensate', default='true')
scan_mode = LaunchConfiguration('scan_mode', default='Standard')
return LaunchDescription([
DeclareLaunchArgument(
'channel_type',
default_value=channel_type,
description='Specifying channel type of lidar'),
DeclareLaunchArgument(
'serial_port',
default_value=serial_port,
description='Specifying usb port to connected lidar'),
DeclareLaunchArgument(
'serial_baudrate',
default_value=serial_baudrate,
description='Specifying usb port baudrate to connected lidar'),
DeclareLaunchArgument(
'frame_id',
default_value=frame_id,
description='Specifying frame_id of lidar'),
DeclareLaunchArgument(
'inverted',
default_value=inverted,
description='Specifying whether or not to invert scan data'),
DeclareLaunchArgument(
'angle_compensate',
default_value=angle_compensate,
description='Specifying whether or not to enable angle_compensate of scan data'),
DeclareLaunchArgument(
'scan_mode',
default_value=scan_mode,
description='Specifying scan mode of lidar'),
Node(
package='rplidar_ros',
executable='rplidar_node',
name='rplidar_node',
parameters=[{'channel_type':channel_type,
'serial_port': serial_port,
'serial_baudrate': serial_baudrate,
'frame_id': frame_id,
'inverted': inverted,
'angle_compensate': angle_compensate,
'scan_mode': scan_mode}],
output='screen'),
])
================================================
FILE: lidarbot_bringup/mpu6050_hardware.xml
================================================
<library path="mpu6050_hardware_interface">
<class name="lidarbot_bringup/MPU6050Hardware"
type="lidarbot_bringup::MPU6050Hardware"
base_class_type="hardware_interface::SensorInterface">
<description>
The ros2_control hardware interface for the MPU6050 inertial measurement unit (IMU) sensor.
</description>
</class>
</library>
================================================
FILE: lidarbot_bringup/package.xml
================================================
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>lidarbot_bringup</name>
<version>0.0.0</version>
<description>Lidarbot bringup package</description>
<maintainer email="noobinventor@tutamail.com">Chinedu Amadi</maintainer>
<license>CC-BY-SA-4.0 license </license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>theora_image_transport</depend>
<depend>image_transport_plugins</depend>
<exec_depend>robot_localization</exec_depend>
<exec_depend>rplidar_ros</exec_depend>
<exec_depend>twist_mux</exec_depend>
<exec_depend>v4l2_camera</exec_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
================================================
FILE: lidarbot_bringup/src/mpu6050_covariances.cpp
================================================
// MPU6050 covariance program
// Currently only variances are considered for the imu covariance arrays
#include "lidarbot_bringup/mpu6050_lib.h"
MPU6050 device(0x68);
// Function to find mean
float mean(float array[], int n) {
float sum = 0.0f;
for (int i = 0; i < n; i++)
sum += array[i];
return sum / n;
}
// Function to find variance
float variance(float array[], int n) {
float sum = 0.0f;
float array_mean = mean(array, n);
for (int i = 0; i < n; i++)
sum += (array[i] - array_mean) * (array[i] - array_mean);
return sum / (n - 1);
}
int main() {
// Initialize variables and arrays
float roll, pitch, yaw; // Angle values
float gx, gy, gz; // Gyro values
float ax, ay, az; // Accel values
int sample_size = 500; // Sample data size to calculate variance on
float orient_var[3]; // Orientation variances
float ang_vel_var[3]; // Angular velocity variances
float lin_accel_var[3]; // Linear acceleration variances
float roll_array[sample_size]; // Array of roll values
float pitch_array[sample_size]; // Array of pitch values
float yaw_array[sample_size]; // Array of yaw values
float ang_vel_x_array[sample_size]; // Array of angular velocity values in x-axis
float ang_vel_y_array[sample_size]; // Array of angular velocity values in y-axis
float ang_vel_z_array[sample_size]; // Array of angular velocity values in z-axis
float lin_accel_x_array[sample_size]; // Array of linear acceleration values in x-axis
float lin_accel_y_array[sample_size]; // Array of linear acceleration values in y-axis
float lin_accel_z_array[sample_size]; // Array of linear acceleration values in z-axis
sleep(1); // Wait for the MPU6050 to stabilize
device.calc_yaw = true;
std::cout << "\nPlease keep the MPU6050 module level and still. \n";
std::cout << "Reading and appending " << sample_size << " sensor data points to respective arrays" << ", it may take a while ... \n\n";
// Read and append sensor data to arrays
int i; // loop variable
for (i = 0; i < sample_size; i++) {
// Read roll, pitch and yaw angles
device.getAngle(0, &roll);
device.getAngle(1, &pitch);
device.getAngle(2, &yaw);
roll_array[i] = roll;
pitch_array[i] = pitch;
yaw_array[i] = yaw;
// Read gyro values
device.getGyro(&gx, &gy, &gz);
ang_vel_x_array[i] = gx;
ang_vel_y_array[i] = gy;
ang_vel_z_array[i] = gz;
// Read accel values
device.getAccel(&ax, &ay, &az);
lin_accel_x_array[i] = ax;
lin_accel_y_array[i] = ay;
lin_accel_z_array[i] = az;
usleep(250000); // 0.25sec delay to read non-consecutive values
}
std::cout << "Calculating variances ...\n\n";
sleep(1);
// Calculate variances
orient_var[0] = variance(roll_array, sample_size);
orient_var[1] = variance(pitch_array, sample_size);
orient_var[2] = variance(yaw_array, sample_size);
ang_vel_var[0] = variance(ang_vel_x_array, sample_size);
ang_vel_var[1] = variance(ang_vel_y_array, sample_size);
ang_vel_var[2] = variance(ang_vel_z_array, sample_size);
lin_accel_var[0] = variance(lin_accel_x_array, sample_size);
lin_accel_var[1] = variance(lin_accel_y_array, sample_size);
lin_accel_var[2] = variance(lin_accel_z_array, sample_size);
// Output variance
std::cout << "static_covariance_orientation: [" << orient_var[0] << ", 0.0, 0.0, 0.0, "
<< orient_var[1] << ", 0.0, 0.0, 0.0, " << orient_var[2] << "]\n";
std::cout << "static_covariance_angular_velocity: [" << ang_vel_var[0] << ", 0.0, 0.0, 0.0, "
<< ang_vel_var[1] << ", 0.0, 0.0, 0.0, " << ang_vel_var[2] << "]\n";
std::cout << "static_covariance_linear_acceleration: [" << lin_accel_var[0] << ", 0.0, 0.0, 0.0, "
<< lin_accel_var[1] << ", 0.0, 0.0, 0.0, " << lin_accel_var[2] << "]\n\n";
std::cout << "Paste covariance arrays in the imu_broadcaster ros__parameters section in lidarbot_bringup/config/controllers.yaml.\n";
return 0;
}
================================================
FILE: lidarbot_bringup/src/mpu6050_hardware_interface.cpp
================================================
#include "lidarbot_bringup/mpu6050_hardware_interface.hpp"
namespace lidarbot_bringup
{
// Initialize MPU6050 device
MPU6050 device(0x68);
MPU6050Hardware::MPU6050Hardware()
: logger_(rclcpp::get_logger("MPU6050Hardware"))
{}
CallbackReturn MPU6050Hardware::on_init(const hardware_interface::HardwareInfo & info)
{
if (hardware_interface::SensorInterface::on_init(info) != CallbackReturn::SUCCESS)
{
return CallbackReturn::ERROR;
}
RCLCPP_INFO(logger_, "Initializing...");
RCLCPP_INFO(logger_, "Finished initialization");
return CallbackReturn::SUCCESS;
}
std::vector<hardware_interface::StateInterface> MPU6050Hardware::export_state_interfaces()
{
// Set up the MPU6050 state interfaces
std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.emplace_back(hardware_interface::StateInterface(info_.sensors[0].name, info_.sensors[0].state_interfaces[0].name, &orientation.x));
state_interfaces.emplace_back(hardware_interface::StateInterface(info_.sensors[0].name, info_.sensors[0].state_interfaces[1].name, &orientation.y));
state_interfaces.emplace_back(hardware_interface::StateInterface(info_.sensors[0].name, info_.sensors[0].state_interfaces[2].name, &orientation.z));
state_interfaces.emplace_back(hardware_interface::StateInterface(info_.sensors[0].name, info_.sensors[0].state_interfaces[3].name, &orientation.w));
state_interfaces.emplace_back(hardware_interface::StateInterface(info_.sensors[0].name, info_.sensors[0].state_interfaces[4].name, &angular_vel_x));
state_interfaces.emplace_back(hardware_interface::StateInterface(info_.sensors[0].name, info_.sensors[0].state_interfaces[5].name, &angular_vel_y));
state_interfaces.emplace_back(hardware_interface::StateInterface(info_.sensors[0].name, info_.sensors[0].state_interfaces[6].name, &angular_vel_z));
state_interfaces.emplace_back(hardware_interface::StateInterface(info_.sensors[0].name, info_.sensors[0].state_interfaces[7].name, &linear_accel_x));
state_interfaces.emplace_back(hardware_interface::StateInterface(info_.sensors[0].name, info_.sensors[0].state_interfaces[8].name, &linear_accel_y));
state_interfaces.emplace_back(hardware_interface::StateInterface(info_.sensors[0].name, info_.sensors[0].state_interfaces[9].name, &linear_accel_z));
return state_interfaces;
}
CallbackReturn MPU6050Hardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
{
RCLCPP_INFO(logger_, "Starting controller ...");
return CallbackReturn::SUCCESS;
}
CallbackReturn MPU6050Hardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/)
{
RCLCPP_INFO(logger_, "Stopping Controller...");
return CallbackReturn::SUCCESS;
}
return_type MPU6050Hardware::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
{
// Obtain current euler angles
device.getAngle(0, &euler_angles[0]);
device.getAngle(1, &euler_angles[1]);
device.getAngle(2, &euler_angles[2]);
// Obtain current quaternion from euler angles
quat = device.getQuat(&euler_angles[0], &euler_angles[1], &euler_angles[2]);
// Obtain current gyroscope and accelerometer values
device.getGyro(&gyro_values[0], &gyro_values[1], &gyro_values[2]);
device.getAccel(&accel_values[0], &accel_values[1], &accel_values[2]);
// Assign values to the state interfaces
// Orientation, angular velocity and linear acceleration conform the East North Up (ENU) coordinate frame
// convention (https://www.ros.org/reps/rep-0103.html) required by the robot_localization package
orientation.x = quat.y;
orientation.y = quat.x;
orientation.z = quat.z;
orientation.w = quat.w;
angular_vel_x = (double)gyro_values[1];
angular_vel_y = (double)gyro_values[0];
angular_vel_z = (double)gyro_values[2];
linear_accel_x = (double)accel_values[1];
linear_accel_y = (double)accel_values[0];
linear_accel_z = (double)accel_values[2];
return return_type::OK;
}
} // namespace lidarbot_bringup
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(
lidarbot_bringup::MPU6050Hardware,
hardware_interface::SensorInterface)
================================================
FILE: lidarbot_bringup/src/mpu6050_lib.cpp
================================================
//-------------------------------MPU6050 Accelerometer and Gyroscope C++ library-----------------------------
//Made changes to class and renamed file. Adapted from Alex Mous (https://github.com/alex-mous/MPU6050-C-CPP-Library-for-Raspberry-Pi)
//Copyright (c) 2019, Alex Mous
//Licensed under the CC BY-NC SA 4.0
#include "lidarbot_bringup/mpu6050_lib.h"
MPU6050::MPU6050(int8_t addr, bool run_update_thread) {
int status;
MPU6050_addr = addr;
dt = 0.009; //Loop time (recalculated with each loop)
_first_run = 1; //Variable for whether to set gyro angle to acceleration angle in compFilter
calc_yaw = false;
f_dev = open("/dev/i2c-1", O_RDWR); //Open the I2C device file
if (f_dev < 0) { //Catch errors
std::cout << "ERR (MPU6050.cpp:MPU6050()): Failed to open /dev/i2c-1. Please check that I2C is enabled with raspi-config\n"; //Print error message
}
status = ioctl(f_dev, I2C_SLAVE, MPU6050_addr); //Set the I2C bus to use the correct address
if (status < 0) {
std::cout << "ERR (MPU6050.cpp:MPU6050()): Could not get I2C bus with " << addr << " address. Please confirm that this address is correct\n"; //Print error message
}
i2c_smbus_write_byte_data(f_dev, 0x6b, 0b00000000); //Take MPU6050 out of sleep mode - see Register Map
i2c_smbus_write_byte_data(f_dev, 0x1a, 0b00000011); //Set DLPF (low pass filter) to 44Hz (so no noise above 44Hz will pass through)
i2c_smbus_write_byte_data(f_dev, 0x19, 0b00000100); //Set sample rate divider (to 200Hz) - see Register Map
i2c_smbus_write_byte_data(f_dev, 0x1b, GYRO_CONFIG); //Configure gyroscope settings - see Register Map (see MPU6050.h for the GYRO_CONFIG parameter)
i2c_smbus_write_byte_data(f_dev, 0x1c, ACCEL_CONFIG); //Configure accelerometer settings - see Register Map (see MPU6050.h for the GYRO_CONFIG parameter)
//Set offsets to zero
i2c_smbus_write_byte_data(f_dev, 0x06, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x07, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x08, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x09, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x0A, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x0B, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x00, 0b10000001), i2c_smbus_write_byte_data(f_dev, 0x01, 0b00000001), i2c_smbus_write_byte_data(f_dev, 0x02, 0b10000001);
if (run_update_thread){
std::thread(&MPU6050::_update, this).detach(); //Create a seperate thread, for the update routine to run in the background, and detach it, allowing the program to continue
}
}
MPU6050::MPU6050(int8_t addr) : MPU6050(addr, true){}
void MPU6050::getGyroRaw(float *roll, float *pitch, float *yaw) {
int16_t X = i2c_smbus_read_byte_data(f_dev, 0x43) << 8 | i2c_smbus_read_byte_data(f_dev, 0x44); //Read X registers
int16_t Y = i2c_smbus_read_byte_data(f_dev, 0x45) << 8 | i2c_smbus_read_byte_data(f_dev, 0x46); //Read Y registers
int16_t Z = i2c_smbus_read_byte_data(f_dev, 0x47) << 8 | i2c_smbus_read_byte_data(f_dev, 0x48); //Read Z registers
*roll = (float)X; //Roll on X axis
*pitch = (float)Y; //Pitch on Y axis
*yaw = (float)Z; //Yaw on Z axis
}
void MPU6050::getGyro(float *roll, float *pitch, float *yaw) {
getGyroRaw(roll, pitch, yaw); //Store raw values into variables
*roll = round((*roll - G_OFF_X) * 1000.0 / GYRO_SENS) / 1000.0; //Remove the offset and divide by the gyroscope sensetivity (use 1000 and round() to round the value to three decimal places)
*pitch = round((*pitch - G_OFF_Y) * 1000.0 / GYRO_SENS) / 1000.0;
*yaw = round((*yaw - G_OFF_Z) * 1000.0 / GYRO_SENS) / 1000.0;
// Convert deg/s to rad/s
*roll *= (M_PI / 180);
*pitch *= (M_PI / 180);
*yaw *= (M_PI / 180);
}
void MPU6050::getAccelRaw(float *x, float *y, float *z) {
int16_t X = i2c_smbus_read_byte_data(f_dev, 0x3b) << 8 | i2c_smbus_read_byte_data(f_dev, 0x3c); //Read X registers
int16_t Y = i2c_smbus_read_byte_data(f_dev, 0x3d) << 8 | i2c_smbus_read_byte_data(f_dev, 0x3e); //Read Y registers
int16_t Z = i2c_smbus_read_byte_data(f_dev, 0x3f) << 8 | i2c_smbus_read_byte_data(f_dev, 0x40); //Read Z registers
*x = (float)X;
*y = (float)Y;
*z = (float)Z;
}
void MPU6050::getAccel(float *x, float *y, float *z) {
getAccelRaw(x, y, z); //Store raw values into variables
*x = round((*x - A_OFF_X) * 1000.0 / ACCEL_SENS) / 1000.0; //Remove the offset and divide by the accelerometer sensetivity (use 1000 and round() to round the value to three decimal places)
*y = round((*y - A_OFF_Y) * 1000.0 / ACCEL_SENS) / 1000.0;
*z = round((*z - A_OFF_Z) * 1000.0 / ACCEL_SENS) / 1000.0;
// Convert g-unit to m/s^2
*x *= M_PER_SECS_SQUARED;
*y *= M_PER_SECS_SQUARED;
*z *= M_PER_SECS_SQUARED;
}
void MPU6050::getOffsets(float *ax_off, float *ay_off, float *az_off, float *gr_off, float *gp_off, float *gy_off) {
float gyro_off[3]; //Temporary storage
float accel_off[3];
*gr_off = 0, *gp_off = 0, *gy_off = 0; //Initialize the offsets to zero
*ax_off = 0, *ay_off = 0, *az_off = 0; //Initialize the offsets to zero
for (int i = 0; i < 10000; i++) { //Use loop to average offsets
getGyroRaw(&gyro_off[0], &gyro_off[1], &gyro_off[2]); //Raw gyroscope values
*gr_off = *gr_off + gyro_off[0], *gp_off = *gp_off + gyro_off[1], *gy_off = *gy_off + gyro_off[2]; //Add to sum
getAccelRaw(&accel_off[0], &accel_off[1], &accel_off[2]); //Raw accelerometer values
*ax_off = *ax_off + accel_off[0], *ay_off = *ay_off + accel_off[1], *az_off = *az_off + accel_off[2]; //Add to sum
}
*gr_off = *gr_off / 10000, *gp_off = *gp_off / 10000, *gy_off = *gy_off / 10000; //Divide by number of loops (to average)
*ax_off = *ax_off / 10000, *ay_off = *ay_off / 10000, *az_off = *az_off / 10000;
*az_off = *az_off - ACCEL_SENS; //Remove 1g from the value calculated to compensate for gravity)
}
// Get quaternion values from euler angles
Quaternion MPU6050::getQuat(float *roll, float *pitch, float *yaw) {
Quaternion quat; // Temporary storage for quaternion
float cr = cos(*roll * 0.5);
float sr = sin(*roll * 0.5);
float cp = cos(*pitch * 0.5);
float sp = sin(*pitch * 0.5);
float cy = cos(*yaw * 0.5);
float sy = sin(*yaw * 0.5);
quat.x = sr * cp * cy - cr * sp * sy;
quat.y = cr * sp * cy + sr * cp * sy;
quat.z = cr * cp * sy - sr * sp * cy;
quat.w = cr * cp * cy + sr * sp * sy;
return quat;
}
int MPU6050::getAngle(int axis, float *result) {
if (axis >= 0 && axis <= 2) { //Check that the axis is in the valid range
*result = _angle[axis]; //Get the result
*result *= (M_PI / 180); // Convert from deg to rad
return 0;
}
else {
std::cout << "ERR (MPU6050.cpp:getAngle()): 'axis' must be between 0 and 2 (for roll, pitch or yaw)\n"; //Print error message
*result = 0; //Set result to zero
return 1;
}
}
void MPU6050::_update() { //Main update function - runs continuously
clock_gettime(CLOCK_REALTIME, &start); //Read current time into start variable
while (1) { //Loop forever
getGyro(&gr, &gp, &gy); //Get the data from the sensors
getAccel(&ax, &ay, &az);
//X (roll) axis
_accel_angle[0] = atan2(az, ay) * RAD_T_DEG - 90.0; //Calculate the angle with z and y convert to degrees and subtract 90 degrees to rotate
_gyro_angle[0] = _angle[0] + gr*dt; //Use roll axis (X axis)
//Y (pitch) axis
_accel_angle[1] = atan2(az, ax) * RAD_T_DEG - 90.0; //Calculate the angle with z and x convert to degrees and subtract 90 degrees to rotate
_gyro_angle[1] = _angle[1] + gp*dt; //Use pitch axis (Y axis)
//Z (yaw) axis
if (calc_yaw) {
_gyro_angle[2] = _angle[2] + gy*dt; //Use yaw axis (Z axis)
}
if (_first_run) { //Set the gyroscope angle reference point if this is the first function run
for (int i = 0; i <= 1; i++) {
_gyro_angle[i] = _accel_angle[i]; //Start off with angle from accelerometer (absolute angle since gyroscope is relative)
}
_gyro_angle[2] = 0; //Set the yaw axis to zero (because the angle cannot be calculated with the accelerometer when vertical)
_first_run = 0;
}
float asum = abs(ax) + abs(ay) + abs(az); //Calculate the sum of the accelerations
float gsum = abs(gr) + abs(gp) + abs(gy); //Calculate the sum of the gyro readings
for (int i = 0; i <= 1; i++) { //Loop through roll and pitch axes
if (abs(_gyro_angle[i] - _accel_angle[i]) > 5) { //Correct for very large drift (or incorrect measurment of gyroscope by longer loop time)
_gyro_angle[i] = _accel_angle[i];
}
//Create result from either complementary filter or directly from gyroscope or accelerometer depending on conditions
if (asum > 0.1 && asum < 3 && gsum > 0.3) { //Check that th movement is not very high (therefore providing inacurate angles)
_angle[i] = (1 - TAU)*(_gyro_angle[i]) + (TAU)*(_accel_angle[i]); //Calculate the angle using a complementary filter
}
else if (gsum > 0.3) { //Use the gyroscope angle if the acceleration is high
_angle[i] = _gyro_angle[i];
}
else if (gsum <= 0.3) { //Use accelerometer angle if not much movement
_angle[i] = _accel_angle[i];
}
}
//The yaw axis will not work with the accelerometer angle, so only use gyroscope angle
if (calc_yaw) { //Only calculate the angle when we want it to prevent large drift
_angle[2] = _gyro_angle[2];
}
else {
_angle[2] = 0;
_gyro_angle[2] = 0;
}
clock_gettime(CLOCK_REALTIME, &end); //Save time to end clock
dt = (end.tv_sec - start.tv_sec) + (end.tv_nsec - start.tv_nsec) / 1e9; //Calculate new dt
clock_gettime(CLOCK_REALTIME, &start); //Save time to start clock
}
}
================================================
FILE: lidarbot_bringup/src/mpu6050_offsets.cpp
================================================
// MPU6050 offsets program
// Adapted from Example.cpp at https://github.com/alex-mous/MPU6050-C-CPP-Library-for-Raspberry-Pi
#include "lidarbot_bringup/mpu6050_lib.h"
MPU6050 device(0x68);
int main() {
float ax, ay, az, gx, gy, gz; // Variables to store the accel and gyro values
sleep(1); // Wait for the MPU6050 to stabilize
// Calculate the offsets
std::cout << "\nPlease keep the MPU6050 module level and still. This may take a few minutes.\n\n";
std::cout << "Calculating offsets ...\n\n";
device.getOffsets(&ax, &ay, &az, &gx, &gy, &gz);
std::cout << "Gyroscope offsets:\n";
std::cout << "------------------\n";
std::cout << "X: " << gx << "\n";
std::cout << "Y: " << gy << "\n";
std::cout << "Z: " << gz << "\n\n";
std::cout << "Accelerometer offsets:\n";
std::cout << "----------------------\n";
std::cout << "X: " << ax << "\n";
std::cout << "Y: " << ay << "\n";
std::cout << "Z: " << az << "\n\n";
std::cout << "Include the obtained offsets in the respective macros of the mpu6050_lib.h file.\n";
return 0;
}
================================================
FILE: lidarbot_description/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 3.8)
project(lidarbot_description)
find_package(ament_cmake REQUIRED)
find_package(urdf REQUIRED)
install(
DIRECTORY launch meshes urdf rviz
DESTINATION share/${PROJECT_NAME}
)
ament_package()
================================================
FILE: lidarbot_description/launch/description_launch.py
================================================
# Launch lidarbot URDF file using Rviz
# File adapted from https://automaticaddison.com
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# Set the path to different files and folders
pkg_path= FindPackageShare(package='lidarbot_description').find('lidarbot_description')
rviz_config_path = os.path.join(pkg_path, 'rviz/description.rviz')
urdf_model_path = os.path.join(pkg_path, 'urdf/lidarbot.urdf.xacro')
# Launch configuration variables specific to simulation
gui = LaunchConfiguration('use_gui')
urdf_model = LaunchConfiguration('urdf_model')
rviz_config_file = LaunchConfiguration('rviz_config_file')
use_sim_time = LaunchConfiguration('use_sim_time')
# Declare the launch arguments
declare_urdf_model_path_cmd = DeclareLaunchArgument(
name='urdf_model',
default_value=urdf_model_path,
description='Absolute path to robot urdf file')
declare_rviz_config_file_cmd = DeclareLaunchArgument(
name='rviz_config_file',
default_value=rviz_config_path,
description='Full path to the RVIZ config file to use')
declare_use_sim_time_cmd = DeclareLaunchArgument(
name='use_sim_time',
default_value='False',
description='Use simulation (Gazebo) clock if true')
declare_use_joint_state_publisher_gui_cmd = DeclareLaunchArgument(
name='use_gui',
default_value='False',
description='Flag to enable joint_state_publisher_gui')
# Publish the joint state values for the non-fixed joints in the URDF file.
start_joint_state_publisher_cmd = Node(
condition=UnlessCondition(gui),
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher')
# A GUI to manipulate the joint state values
start_joint_state_publisher_gui_node = Node(
condition=IfCondition(gui),
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
name='joint_state_publisher_gui')
# Start robot state publisher
start_robot_state_publisher_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(pkg_path, 'launch', 'robot_state_publisher_launch.py')]),
launch_arguments={'use_sim_time': use_sim_time, 'urdf_model': urdf_model}.items())
# Launch RViz
start_rviz_cmd = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', rviz_config_file])
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_urdf_model_path_cmd)
ld.add_action(declare_rviz_config_file_cmd)
ld.add_action(declare_use_joint_state_publisher_gui_cmd)
gitextract_xh4fad55/
├── .github/
│ └── workflows/
│ └── lidarbot_ci_action.yml
├── .gitignore
├── LICENSE
├── README.md
├── docs/
│ └── index.md
├── lidarbot/
│ ├── CMakeLists.txt
│ └── package.xml
├── lidarbot_aruco/
│ ├── config/
│ │ ├── chessboard_calibration.yaml
│ │ ├── params_1.yaml
│ │ └── params_2.yaml
│ ├── launch/
│ │ └── trajectory_visualizer_launch.py
│ ├── lidarbot_aruco/
│ │ ├── __init__.py
│ │ ├── aruco_trajectory_visualizer.py
│ │ ├── detect_aruco_marker.py
│ │ └── generate_aruco_marker.py
│ ├── package.xml
│ ├── resource/
│ │ └── lidarbot_aruco
│ ├── setup.cfg
│ ├── setup.py
│ └── test/
│ ├── test_copyright.py
│ ├── test_flake8.py
│ └── test_pep257.py
├── lidarbot_base/
│ ├── CMakeLists.txt
│ ├── include/
│ │ └── lidarbot_base/
│ │ ├── DEV_Config.h
│ │ ├── Debug.h
│ │ ├── MotorDriver.h
│ │ ├── PCA9685.h
│ │ ├── lidarbot_hardware.hpp
│ │ ├── motor_encoder.h
│ │ └── wheel.hpp
│ ├── lidarbot_hardware.xml
│ ├── package.xml
│ └── src/
│ ├── DEV_Config.c
│ ├── MotorDriver.c
│ ├── PCA9685.c
│ ├── lidarbot_hardware.cpp
│ ├── motor_checks_client.cpp
│ ├── motor_checks_server.cpp
│ ├── motor_encoder.c
│ └── wheel.cpp
├── lidarbot_bringup/
│ ├── CMakeLists.txt
│ ├── LICENSE
│ ├── config/
│ │ ├── controllers.yaml
│ │ └── gazebo_ros2_ctl_use_sim.yaml
│ ├── include/
│ │ └── lidarbot_bringup/
│ │ ├── mpu6050_hardware_interface.hpp
│ │ └── mpu6050_lib.h
│ ├── launch/
│ │ ├── camera_launch.py
│ │ ├── lidarbot_bringup_launch.py
│ │ └── rplidar_launch.py
│ ├── mpu6050_hardware.xml
│ ├── package.xml
│ └── src/
│ ├── mpu6050_covariances.cpp
│ ├── mpu6050_hardware_interface.cpp
│ ├── mpu6050_lib.cpp
│ └── mpu6050_offsets.cpp
├── lidarbot_description/
│ ├── CMakeLists.txt
│ ├── launch/
│ │ ├── description_launch.py
│ │ └── robot_state_publisher_launch.py
│ ├── meshes/
│ │ ├── MPU6050.dae
│ │ ├── caster_mount.dae
│ │ ├── caster_wheel.dae
│ │ ├── power_bank.dae
│ │ ├── power_bank_stand.dae
│ │ ├── right_wheel.dae
│ │ ├── robot_chassis.dae
│ │ ├── rpi_4.dae
│ │ ├── rpi_camera.dae
│ │ ├── rpi_camera_mount.dae
│ │ ├── rpi_stand.dae
│ │ ├── rplidar_base.dae
│ │ ├── rplidar_stand.dae
│ │ └── rplidar_top.dae
│ ├── package.xml
│ ├── rviz/
│ │ ├── description.rviz
│ │ └── lidarbot_sim.rviz
│ └── urdf/
│ ├── camera.xacro
│ ├── gazebo_control.xacro
│ ├── imu.xacro
│ ├── inertial_macros.xacro
│ ├── lidar.xacro
│ ├── lidarbot.urdf.xacro
│ ├── lidarbot_core.xacro
│ └── ros2_control.xacro
├── lidarbot_gazebo/
│ ├── CMakeLists.txt
│ ├── config/
│ │ └── gazebo_params.yaml
│ ├── launch/
│ │ └── gazebo_launch.py
│ ├── package.xml
│ └── worlds/
│ └── obstacles.world
├── lidarbot_gz/
│ ├── CMakeLists.txt
│ ├── config/
│ │ └── lidarbot_bridge.yaml
│ ├── launch/
│ │ └── gz_launch.py
│ ├── models/
│ │ ├── construction_barrel/
│ │ │ ├── meshes/
│ │ │ │ └── construction_barrel.dae
│ │ │ ├── model-1_3.sdf
│ │ │ ├── model-1_4.sdf
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ └── construction_cone/
│ │ ├── meshes/
│ │ │ └── construction_cone.dae
│ │ ├── model-1_3.sdf
│ │ ├── model-1_4.sdf
│ │ ├── model.config
│ │ └── model.sdf
│ ├── package.xml
│ ├── urdf/
│ │ ├── lidarbot_gz.urdf.xacro
│ │ ├── lidarbot_gz_core.xacro
│ │ └── sensors.xacro
│ └── worlds/
│ └── obstacles.sdf
├── lidarbot_navigation/
│ ├── CMakeLists.txt
│ ├── config/
│ │ ├── ekf.yaml
│ │ └── nav2_params.yaml
│ ├── launch/
│ │ ├── localization_launch.py
│ │ └── navigation_launch.py
│ ├── lidarbot_navigation/
│ │ └── __init__.py
│ ├── maps/
│ │ ├── real_map3.pgm
│ │ ├── real_map3.yaml
│ │ ├── sim_map.data
│ │ ├── sim_map.pgm
│ │ ├── sim_map.posegraph
│ │ └── sim_map.yaml
│ ├── package.xml
│ ├── rviz/
│ │ └── lidarbot_nav.rviz
│ └── scripts/
│ └── trajectory_visualizer.py
├── lidarbot_slam/
│ ├── CMakeLists.txt
│ ├── config/
│ │ └── mapper_params_online_async.yaml
│ ├── launch/
│ │ └── online_async_launch.py
│ ├── package.xml
│ └── rviz/
│ └── lidarbot_slam.rviz
└── lidarbot_teleop/
├── CMakeLists.txt
├── config/
│ ├── joystick.yaml
│ └── twist_mux.yaml
├── launch/
│ └── joystick_launch.py
└── package.xml
SYMBOL INDEX (89 symbols across 35 files)
FILE: lidarbot_aruco/launch/trajectory_visualizer_launch.py
function generate_launch_description (line 11) | def generate_launch_description():
FILE: lidarbot_aruco/lidarbot_aruco/aruco_trajectory_visualizer.py
class ArucoNode (line 41) | class ArucoNode(Node):
method __init__ (line 42) | def __init__(self):
method listener_callback (line 116) | def listener_callback(self, data):
function main (line 182) | def main(args=None):
FILE: lidarbot_aruco/lidarbot_aruco/detect_aruco_marker.py
function main (line 43) | def main():
FILE: lidarbot_aruco/lidarbot_aruco/generate_aruco_marker.py
function main (line 57) | def main():
FILE: lidarbot_aruco/test/test_copyright.py
function test_copyright (line 23) | def test_copyright():
FILE: lidarbot_aruco/test/test_flake8.py
function test_flake8 (line 21) | def test_flake8():
FILE: lidarbot_aruco/test/test_pep257.py
function test_pep257 (line 21) | def test_pep257():
FILE: lidarbot_base/include/lidarbot_base/MotorDriver.h
type DIR (line 33) | typedef enum {
FILE: lidarbot_base/include/lidarbot_base/lidarbot_hardware.hpp
type lidarbot_base (line 26) | namespace lidarbot_base
class LidarbotHardware (line 29) | class LidarbotHardware : public hardware_interface::SystemInterface
type Config (line 32) | struct Config
FILE: lidarbot_base/include/lidarbot_base/wheel.hpp
class Wheel (line 7) | class Wheel
method Wheel (line 17) | Wheel() = default;
FILE: lidarbot_base/src/DEV_Config.c
function DEV_Equipment_Testing (line 23) | static int DEV_Equipment_Testing(void)
function DEV_GPIO_Mode (line 71) | void DEV_GPIO_Mode(UWORD Pin, UWORD Mode)
function DEV_Digital_Write (line 100) | void DEV_Digital_Write(UWORD Pin, UBYTE Value)
function UBYTE (line 114) | UBYTE DEV_Digital_Read(UWORD Pin)
function DEV_Delay_ms (line 133) | void DEV_Delay_ms(UDOUBLE xms)
function GPIO_Config (line 148) | void GPIO_Config(void)
function DEV_SPI_Init (line 170) | void DEV_SPI_Init()
function DEV_SPI_WriteByte (line 194) | void DEV_SPI_WriteByte(uint8_t Value)
function DEV_SPI_Write_nByte (line 209) | void DEV_SPI_Write_nByte(uint8_t *pData, uint32_t Len)
function DEV_I2C_Init (line 230) | void DEV_I2C_Init(uint8_t Add)
function I2C_Write_Byte (line 250) | void I2C_Write_Byte(uint8_t Cmd, uint8_t value)
function I2C_Read_Byte (line 273) | int I2C_Read_Byte(uint8_t Cmd)
function I2C_Read_Word (line 295) | int I2C_Read_Word(uint8_t Cmd)
function UBYTE (line 321) | UBYTE DEV_ModuleInit(void)
function DEV_ModuleExit (line 354) | void DEV_ModuleExit(void)
FILE: lidarbot_base/src/MotorDriver.c
function Motor_Init (line 28) | void Motor_Init(void)
function Motor_Run (line 46) | void Motor_Run(UBYTE motor, DIR dir, UWORD speed)
function Motor_Stop (line 95) | void Motor_Stop(UBYTE motor)
function UBYTE (line 115) | UBYTE Motor_Direction(UBYTE motor)
FILE: lidarbot_base/src/PCA9685.c
function PCA9685_WriteByte (line 29) | static void PCA9685_WriteByte(UBYTE reg, UBYTE value)
function UBYTE (line 42) | static UBYTE PCA9685_ReadByte(UBYTE reg)
function PCA9685_SetPWM (line 58) | static void PCA9685_SetPWM(UBYTE channel, UWORD on, UWORD off)
function PCA9685_Init (line 76) | void PCA9685_Init(char addr)
function PCA9685_SetPWMFreq (line 96) | void PCA9685_SetPWMFreq(UWORD freq)
function PCA9685_SetPwmDutyCycle (line 127) | void PCA9685_SetPwmDutyCycle(UBYTE channel, UWORD pulse)
function PCA9685_SetLevel (line 141) | void PCA9685_SetLevel(UBYTE channel, UWORD value)
FILE: lidarbot_base/src/lidarbot_hardware.cpp
type lidarbot_base (line 3) | namespace lidarbot_base
function CallbackReturn (line 10) | CallbackReturn LidarbotHardware::on_init(const hardware_interface::Har...
function CallbackReturn (line 59) | CallbackReturn LidarbotHardware::on_configure(const rclcpp_lifecycle::...
function CallbackReturn (line 88) | CallbackReturn LidarbotHardware::on_activate(const rclcpp_lifecycle::S...
function CallbackReturn (line 95) | CallbackReturn LidarbotHardware::on_deactivate(const rclcpp_lifecycle:...
function return_type (line 102) | return_type LidarbotHardware::read(const rclcpp::Time & /*time*/, cons...
function return_type (line 122) | return_type LidarbotHardware::write(const rclcpp::Time & /*time*/, con...
FILE: lidarbot_base/src/motor_checks_client.cpp
function main (line 9) | int main(int argc, char **argv) {
FILE: lidarbot_base/src/motor_checks_server.cpp
function reset_pulse_counters (line 8) | void reset_pulse_counters()
function move_motor (line 15) | bool move_motor(int motor_id)
function checkMotors (line 42) | void checkMotors(const std::shared_ptr<std_srvs::srv::Trigger::Request> ...
function main (line 69) | int main(int argc, char **argv)
FILE: lidarbot_base/src/motor_encoder.c
function read_encoder_values (line 17) | void read_encoder_values(int *left_encoder_value, int *right_encoder_val...
function left_wheel_pulse (line 23) | void left_wheel_pulse() {
function right_wheel_pulse (line 38) | void right_wheel_pulse() {
function set_motor_speeds (line 53) | void set_motor_speeds(double left_wheel_command, double right_wheel_comm...
function handler (line 80) | void handler(int signo) {
FILE: lidarbot_bringup/include/lidarbot_bringup/mpu6050_hardware_interface.hpp
type lidarbot_bringup (line 23) | namespace lidarbot_bringup
class MPU6050Hardware (line 26) | class MPU6050Hardware : public hardware_interface::SensorInterface
FILE: lidarbot_bringup/include/lidarbot_bringup/mpu6050_lib.h
type Quaternion (line 90) | struct Quaternion
function class (line 95) | class MPU6050 {
FILE: lidarbot_bringup/launch/camera_launch.py
function generate_launch_description (line 6) | def generate_launch_description():
FILE: lidarbot_bringup/launch/lidarbot_bringup_launch.py
function generate_launch_description (line 25) | def generate_launch_description():
FILE: lidarbot_bringup/launch/rplidar_launch.py
function generate_launch_description (line 12) | def generate_launch_description():
FILE: lidarbot_bringup/src/mpu6050_covariances.cpp
function mean (line 9) | float mean(float array[], int n) {
function variance (line 18) | float variance(float array[], int n) {
function main (line 28) | int main() {
FILE: lidarbot_bringup/src/mpu6050_hardware_interface.cpp
type lidarbot_bringup (line 3) | namespace lidarbot_bringup
function CallbackReturn (line 13) | CallbackReturn MPU6050Hardware::on_init(const hardware_interface::Hard...
function CallbackReturn (line 47) | CallbackReturn MPU6050Hardware::on_activate(const rclcpp_lifecycle::St...
function CallbackReturn (line 54) | CallbackReturn MPU6050Hardware::on_deactivate(const rclcpp_lifecycle::...
function return_type (line 61) | return_type MPU6050Hardware::read(const rclcpp::Time & /*time*/, const...
FILE: lidarbot_bringup/src/mpu6050_lib.cpp
function Quaternion (line 110) | Quaternion MPU6050::getQuat(float *roll, float *pitch, float *yaw) {
FILE: lidarbot_bringup/src/mpu6050_offsets.cpp
function main (line 9) | int main() {
FILE: lidarbot_description/launch/description_launch.py
function generate_launch_description (line 16) | def generate_launch_description():
FILE: lidarbot_description/launch/robot_state_publisher_launch.py
function generate_launch_description (line 12) | def generate_launch_description():
FILE: lidarbot_gazebo/launch/gazebo_launch.py
function generate_launch_description (line 18) | def generate_launch_description():
FILE: lidarbot_gz/launch/gz_launch.py
function generate_launch_description (line 21) | def generate_launch_description():
FILE: lidarbot_navigation/launch/localization_launch.py
function generate_launch_description (line 29) | def generate_launch_description():
FILE: lidarbot_navigation/launch/navigation_launch.py
function generate_launch_description (line 29) | def generate_launch_description():
FILE: lidarbot_navigation/scripts/trajectory_visualizer.py
class TrajectoryVisualizer (line 18) | class TrajectoryVisualizer(Node):
method __init__ (line 20) | def __init__(self, name):
method odom_callback (line 45) | def odom_callback(self, odom_msg):
method publish_trajectory_path (line 52) | def publish_trajectory_path(self, position):
function main (line 84) | def main(args=None):
FILE: lidarbot_slam/launch/online_async_launch.py
function generate_launch_description (line 14) | def generate_launch_description():
FILE: lidarbot_teleop/launch/joystick_launch.py
function generate_launch_description (line 10) | def generate_launch_description():
Copy disabled (too large)
Download .json
Condensed preview — 131 files, each showing path, character count, and a content snippet. Download the .json file for the full structured content (23,003K chars).
[
{
"path": ".github/workflows/lidarbot_ci_action.yml",
"chars": 1146,
"preview": "name: ROS2 CI\n\non:\n pull_request:\n push:\n branches:\n - main\n\njobs:\n build:\n runs-on: ubuntu-22.04\n step"
},
{
"path": ".gitignore",
"chars": 27,
"preview": ".vscode/\n.vale.ini\nstyles/\n"
},
{
"path": "LICENSE",
"chars": 1500,
"preview": "BSD 3-Clause License\n\nCopyright (c) 2023, Chinedu Amadi\n\nRedistribution and use in source and binary forms, with or with"
},
{
"path": "README.md",
"chars": 62012,
"preview": "# Lidarbot\n\n<!-- ![ROS2 CI](https://github.com/TheNoobInventor/lidarbot/actions/workflows/.github/workflows/lidarbot_ci_"
},
{
"path": "docs/index.md",
"chars": 0,
"preview": ""
},
{
"path": "lidarbot/CMakeLists.txt",
"chars": 106,
"preview": "cmake_minimum_required(VERSION 3.8)\nproject(lidarbot)\n\nfind_package(ament_cmake REQUIRED)\n\nament_package()"
},
{
"path": "lidarbot/package.xml",
"chars": 502,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "lidarbot_aruco/config/chessboard_calibration.yaml",
"chars": 690,
"preview": "%YAML:1.0\n---\nimage_width: 640\nimage_height: 480\ncamera_name: narrow_stereo\ncamera_matrix:\n rows: 3\n cols: 3\n dt: d\n "
},
{
"path": "lidarbot_aruco/config/params_1.yaml",
"chars": 644,
"preview": "/**:\n ros__parameters:\n video_device: \"/dev/video0\"\n framerate: 30.0\n io_method: \"mmap\"\n frame_id"
},
{
"path": "lidarbot_aruco/config/params_2.yaml",
"chars": 673,
"preview": "/**:\n ros__parameters:\n video_device: \"/dev/video2\"\n framerate: 15.0\n io_method: \"mmap\"\n frame_id"
},
{
"path": "lidarbot_aruco/launch/trajectory_visualizer_launch.py",
"chars": 836,
"preview": "# Launch file to start the webcam usb driver and aruco trajectory visualizer node\n# to track the robot as it moves\n\nimpo"
},
{
"path": "lidarbot_aruco/lidarbot_aruco/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "lidarbot_aruco/lidarbot_aruco/aruco_trajectory_visualizer.py",
"chars": 7673,
"preview": "#!/usr/bin/env python3\n\n# This node is used to visualize the trajectory of lidarbot by tracking the ArUco marker\n# on it"
},
{
"path": "lidarbot_aruco/lidarbot_aruco/detect_aruco_marker.py",
"chars": 4497,
"preview": "#!/usr/bin/env python\n\n# This detects ArUco markers from a webcam stream using OpenCV and Python\n\n# Adapted from https:/"
},
{
"path": "lidarbot_aruco/lidarbot_aruco/generate_aruco_marker.py",
"chars": 2685,
"preview": "#!/usr/bin/env python\n\n# This script generates ArUco markers using OpenCV and Python\n\n# Adapted from https://automaticad"
},
{
"path": "lidarbot_aruco/package.xml",
"chars": 758,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "lidarbot_aruco/resource/lidarbot_aruco",
"chars": 0,
"preview": ""
},
{
"path": "lidarbot_aruco/setup.cfg",
"chars": 97,
"preview": "[develop]\nscript_dir=$base/lib/lidarbot_aruco\n[install]\ninstall_scripts=$base/lib/lidarbot_aruco\n"
},
{
"path": "lidarbot_aruco/setup.py",
"chars": 1063,
"preview": "import os\nfrom glob import glob\nfrom setuptools import find_packages, setup\n\npackage_name = \"lidarbot_aruco\"\n\nsetup(\n "
},
{
"path": "lidarbot_aruco/test/test_copyright.py",
"chars": 962,
"preview": "# Copyright 2015 Open Source Robotics Foundation, Inc.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\""
},
{
"path": "lidarbot_aruco/test/test_flake8.py",
"chars": 884,
"preview": "# Copyright 2017 Open Source Robotics Foundation, Inc.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\""
},
{
"path": "lidarbot_aruco/test/test_pep257.py",
"chars": 803,
"preview": "# Copyright 2015 Open Source Robotics Foundation, Inc.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\""
},
{
"path": "lidarbot_base/CMakeLists.txt",
"chars": 1993,
"preview": "cmake_minimum_required(VERSION 3.8)\nproject(lidarbot_base)\n\nfind_package(ament_cmake REQUIRED)\nfind_package(hardware_int"
},
{
"path": "lidarbot_base/include/lidarbot_base/DEV_Config.h",
"chars": 1701,
"preview": "#ifndef _DEV_CONFIG_H_\r\n#define _DEV_CONFIG_H_\r\n\r\n#define USE_WIRINGPI_LIB 1\r\n/*****************************************"
},
{
"path": "lidarbot_base/include/lidarbot_base/Debug.h",
"chars": 617,
"preview": "/*****************************************************************************\r\n* | File \t:\tDebug.h\r\n* | Author "
},
{
"path": "lidarbot_base/include/lidarbot_base/MotorDriver.h",
"chars": 1267,
"preview": "/*****************************************************************************\r\n* | File : MotorDriver.h\r\n* | A"
},
{
"path": "lidarbot_base/include/lidarbot_base/PCA9685.h",
"chars": 1734,
"preview": "/*****************************************************************************\r\n* | File : PCA9685.h\r\n* | Autho"
},
{
"path": "lidarbot_base/include/lidarbot_base/lidarbot_hardware.hpp",
"chars": 1939,
"preview": "#ifndef _LIDARBOT_BASE__LIDARBOT_HARDWARE_HPP_\n#define _LIDARBOT_BASE__LIDARBOT_HARDWARE_HPP_\n\n#include <cmath>\n#include"
},
{
"path": "lidarbot_base/include/lidarbot_base/motor_encoder.h",
"chars": 728,
"preview": "#ifdef __cplusplus\nextern \"C\" {\n#endif\n\n#ifndef __MOTOR_ENCODER_H__\n#define __MOTOR_ENCODER_H__\n\n#include <stdio.h>\n#inc"
},
{
"path": "lidarbot_base/include/lidarbot_base/wheel.hpp",
"chars": 500,
"preview": "#ifndef _LIDARBOT_BASE__WHEEL_H_\n#define _LIDARBOT_BASE__WHEEL_H_\n\n#include <string>\n#include <cmath>\n\nclass Wheel\n{ \n"
},
{
"path": "lidarbot_base/lidarbot_hardware.xml",
"chars": 410,
"preview": "<library path=\"lidarbot_hardware\">\n <class name=\"lidarbot_base/LidarbotHardware\"\n type=\"lidarbot_base::Lida"
},
{
"path": "lidarbot_base/package.xml",
"chars": 900,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "lidarbot_base/src/DEV_Config.c",
"chars": 9396,
"preview": "/*****************************************************************************\r\n* | File \t: DEV_Config.c\r\n* | Aut"
},
{
"path": "lidarbot_base/src/MotorDriver.c",
"chars": 3256,
"preview": "/*****************************************************************************\r\n* | File : MotorDriver.c\r\n* | A"
},
{
"path": "lidarbot_base/src/PCA9685.c",
"chars": 4050,
"preview": "/*****************************************************************************\n* | File : PCA9685.c\n* | Author "
},
{
"path": "lidarbot_base/src/lidarbot_hardware.cpp",
"chars": 5252,
"preview": "#include \"lidarbot_base/lidarbot_hardware.hpp\"\n\nnamespace lidarbot_base\n{\n\nLidarbotHardware::LidarbotHardware()\n : lo"
},
{
"path": "lidarbot_base/src/motor_checks_client.cpp",
"chars": 2176,
"preview": "// Client node requests motor checks to be carried out\n\n#include \"rclcpp/rclcpp.hpp\"\n#include \"std_srvs/srv/trigger.hpp\""
},
{
"path": "lidarbot_base/src/motor_checks_server.cpp",
"chars": 3430,
"preview": "// Server node runs motor tests to confirm that the motor is working correctly by moving both motors forward\n\n#include \""
},
{
"path": "lidarbot_base/src/motor_encoder.c",
"chars": 2410,
"preview": "// This file calculate encoder ticks and sets the speed each motor should run at according to \n// the commands received "
},
{
"path": "lidarbot_base/src/wheel.cpp",
"chars": 368,
"preview": "#include \"lidarbot_base/wheel.hpp\"\n\nWheel::Wheel(const std::string &wheel_name, int ticks_per_rev)\n{\n setup(wheel_nam"
},
{
"path": "lidarbot_bringup/CMakeLists.txt",
"chars": 1690,
"preview": "cmake_minimum_required(VERSION 3.8)\nproject(lidarbot_bringup)\n\nset(THIS_PACKAGE_INCLUDE_DEPENDS\n hardware_interface\n p"
},
{
"path": "lidarbot_bringup/LICENSE",
"chars": 20128,
"preview": "Attribution-ShareAlike 4.0 International\n\n=======================================================================\n\nCreat"
},
{
"path": "lidarbot_bringup/config/controllers.yaml",
"chars": 1638,
"preview": "controller_manager: # Node name\n ros__parameters:\n update_rate: 30 # Has to be an integer otherwise errors are encou"
},
{
"path": "lidarbot_bringup/config/gazebo_ros2_ctl_use_sim.yaml",
"chars": 74,
"preview": "controller_manager: # Node name\n ros__parameters:\n use_sim_time: true "
},
{
"path": "lidarbot_bringup/include/lidarbot_bringup/mpu6050_hardware_interface.hpp",
"chars": 1563,
"preview": "#ifndef _LIDARBOT_BRINGUP__MPU6050_HARDWARE_HPP_\n#define _LIDARBOT_BRINGUP__MPU6050_HARDWARE_HPP_\n\n#include <memory>\n#in"
},
{
"path": "lidarbot_bringup/include/lidarbot_bringup/mpu6050_lib.h",
"chars": 3585,
"preview": "//-------------------------------MPU6050 Accelerometer and Gyroscope C++ library-----------------------------\n//Made cha"
},
{
"path": "lidarbot_bringup/launch/camera_launch.py",
"chars": 480,
"preview": "# This launch file runs the v4l2_camera node used to start up the raspberry pi camera v1.3\n\nfrom launch import LaunchDes"
},
{
"path": "lidarbot_bringup/launch/lidarbot_bringup_launch.py",
"chars": 6799,
"preview": "# This launch file brings up the physical lidarbot, raspberry pi camera v1.3,\n# RPLIDAR A1 and also integrates ros2_cont"
},
{
"path": "lidarbot_bringup/launch/rplidar_launch.py",
"chars": 2672,
"preview": "# This launch file runs the rplidar_ros node with the configurations specified\n\n#!/usr/bin/env python3\n\nfrom ament_index"
},
{
"path": "lidarbot_bringup/mpu6050_hardware.xml",
"chars": 347,
"preview": "<library path=\"mpu6050_hardware_interface\">\n\t<class name=\"lidarbot_bringup/MPU6050Hardware\"\n\t\t type=\"lidarbot_bringup:"
},
{
"path": "lidarbot_bringup/package.xml",
"chars": 780,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "lidarbot_bringup/src/mpu6050_covariances.cpp",
"chars": 4320,
"preview": "// MPU6050 covariance program\n// Currently only variances are considered for the imu covariance arrays\n\n#include \"lidarb"
},
{
"path": "lidarbot_bringup/src/mpu6050_hardware_interface.cpp",
"chars": 4063,
"preview": "#include \"lidarbot_bringup/mpu6050_hardware_interface.hpp\"\n\nnamespace lidarbot_bringup\n{\n\n// Initialize MPU6050 device\nM"
},
{
"path": "lidarbot_bringup/src/mpu6050_lib.cpp",
"chars": 9441,
"preview": "//-------------------------------MPU6050 Accelerometer and Gyroscope C++ library-----------------------------\n//Made cha"
},
{
"path": "lidarbot_bringup/src/mpu6050_offsets.cpp",
"chars": 1049,
"preview": "// MPU6050 offsets program\n\n// Adapted from Example.cpp at https://github.com/alex-mous/MPU6050-C-CPP-Library-for-Raspbe"
},
{
"path": "lidarbot_description/CMakeLists.txt",
"chars": 231,
"preview": "cmake_minimum_required(VERSION 3.8)\nproject(lidarbot_description)\n\nfind_package(ament_cmake REQUIRED)\nfind_package(urdf "
},
{
"path": "lidarbot_description/launch/description_launch.py",
"chars": 3502,
"preview": "# Launch lidarbot URDF file using Rviz\n\n# File adapted from https://automaticaddison.com\n\nimport os\n\nfrom launch import "
},
{
"path": "lidarbot_description/launch/robot_state_publisher_launch.py",
"chars": 2182,
"preview": "# Launch file to start the robot state publisher node\n\nimport os\n\nfrom launch import LaunchDescription\nfrom launch.subst"
},
{
"path": "lidarbot_description/meshes/MPU6050.dae",
"chars": 860899,
"preview": "<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n <asset>\n <contributor/>\n <created"
},
{
"path": "lidarbot_description/meshes/caster_mount.dae",
"chars": 815765,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\" xml"
},
{
"path": "lidarbot_description/meshes/caster_wheel.dae",
"chars": 573721,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\" xml"
},
{
"path": "lidarbot_description/meshes/power_bank.dae",
"chars": 25286,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\" xml"
},
{
"path": "lidarbot_description/meshes/power_bank_stand.dae",
"chars": 219937,
"preview": "<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n <asset>\n <contributor/>\n <created"
},
{
"path": "lidarbot_description/meshes/right_wheel.dae",
"chars": 1194426,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "lidarbot_description/meshes/robot_chassis.dae",
"chars": 1105291,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\" xml"
},
{
"path": "lidarbot_description/meshes/rpi_4.dae",
"chars": 5796680,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\" xml"
},
{
"path": "lidarbot_description/meshes/rpi_camera.dae",
"chars": 51890,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\" xml"
},
{
"path": "lidarbot_description/meshes/rpi_camera_mount.dae",
"chars": 214790,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\" xml"
},
{
"path": "lidarbot_description/meshes/rpi_stand.dae",
"chars": 292449,
"preview": "<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n <asset>\n <contributor/>\n <created"
},
{
"path": "lidarbot_description/meshes/rplidar_base.dae",
"chars": 8721889,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "lidarbot_description/meshes/rplidar_stand.dae",
"chars": 219937,
"preview": "<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n <asset>\n <contributor/>\n <created"
},
{
"path": "lidarbot_description/meshes/rplidar_top.dae",
"chars": 2293236,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\" xml"
},
{
"path": "lidarbot_description/package.xml",
"chars": 761,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "lidarbot_description/rviz/description.rviz",
"chars": 7402,
"preview": "Panels:\n - Class: rviz_common/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:"
},
{
"path": "lidarbot_description/rviz/lidarbot_sim.rviz",
"chars": 9106,
"preview": "Panels:\n - Class: rviz_common/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:"
},
{
"path": "lidarbot_description/urdf/camera.xacro",
"chars": 1459,
"preview": "<?xml version=\"1.0\" ?>\n<robot xmlns:xacro=\"http://ros.org/wiki/xacro\">\n\n <!-- Camera -->\n <link name=\"camera_link\">\n "
},
{
"path": "lidarbot_description/urdf/gazebo_control.xacro",
"chars": 959,
"preview": "<?xml version=\"1.0\" ?>\n<robot xmlns:xacro=\"http://ros.org/wiki/xacro\">\n\n <gazebo>\n <plugin name=\"lidarbot_diff"
},
{
"path": "lidarbot_description/urdf/imu.xacro",
"chars": 2476,
"preview": "<?xml version=\"1.0\" ?>\n<robot xmlns:xacro=\"http://ros.org/wiki/xacro\">\n\n <!-- Imu sensor -->\n <link name=\"imu_link\">\n"
},
{
"path": "lidarbot_description/urdf/inertial_macros.xacro",
"chars": 1579,
"preview": "<?xml version=\"1.0\" ?>\n<robot xmlns:xacro=\"http://ros.org/wiki/xacro\">\n\n <!-- Some standard inertial calculations, ob"
},
{
"path": "lidarbot_description/urdf/lidar.xacro",
"chars": 2201,
"preview": "<?xml version=\"1.0\" ?>\n<robot xmlns:xacro=\"http://ros.org/wiki/xacro\">\n\n <link name=\"lidar_link\">\n <visual>\n "
},
{
"path": "lidarbot_description/urdf/lidarbot.urdf.xacro",
"chars": 637,
"preview": "<?xml version=\"1.0\" ?>\n<robot name=\"lidarbot\" xmlns:xacro=\"http://ros.org/wiki/xacro\">\n\n <xacro:arg name=\"use_ros2_co"
},
{
"path": "lidarbot_description/urdf/lidarbot_core.xacro",
"chars": 7869,
"preview": "<?xml version=\"1.0\" ?>\n<robot xmlns:xacro=\"http://ros.org/wiki/xacro\">\n\n <xacro:include filename=\"inertial_macros.xacro"
},
{
"path": "lidarbot_description/urdf/ros2_control.xacro",
"chars": 3133,
"preview": "<?xml version=\"1.0\" ?>\n<robot xmlns:xacro=\"http://ros.org/wiki/xacro\">\n \n <!-- Real robot ros2_control -->\n <xacro:un"
},
{
"path": "lidarbot_gazebo/CMakeLists.txt",
"chars": 222,
"preview": "cmake_minimum_required(VERSION 3.8)\nproject(lidarbot_gazebo)\n\nfind_package(ament_cmake REQUIRED)\nfind_package(urdf REQUI"
},
{
"path": "lidarbot_gazebo/config/gazebo_params.yaml",
"chars": 50,
"preview": "gazebo:\n ros__parameters:\n publish_rate: 400.0"
},
{
"path": "lidarbot_gazebo/launch/gazebo_launch.py",
"chars": 5648,
"preview": "# Launches the lidarbot in Gazebo to be controlled using a joystick. There are a number of launch arguments that can be "
},
{
"path": "lidarbot_gazebo/package.xml",
"chars": 588,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "lidarbot_gazebo/worlds/obstacles.world",
"chars": 28060,
"preview": "<sdf version='1.7'>\n <world name='default'>\n <light name='sun' type='directional'>\n <cast_shadows>1</cast_shado"
},
{
"path": "lidarbot_gz/CMakeLists.txt",
"chars": 232,
"preview": "cmake_minimum_required(VERSION 3.8)\nproject(lidarbot_gz)\n\nfind_package(ament_cmake REQUIRED)\nfind_package(urdf REQUIRED)"
},
{
"path": "lidarbot_gz/config/lidarbot_bridge.yaml",
"chars": 1089,
"preview": "# gz topic published by the simulator core\n- ros_topic_name: \"clock\"\n gz_topic_name: \"clock\"\n ros_type_name: \"rosgraph"
},
{
"path": "lidarbot_gz/launch/gz_launch.py",
"chars": 6761,
"preview": "# Launches the lidarbot in Gazebo Fortress to be controlled using a joystick.\n#\n# File adapted from https://automaticadd"
},
{
"path": "lidarbot_gz/models/construction_barrel/meshes/construction_barrel.dae",
"chars": 165709,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "lidarbot_gz/models/construction_barrel/model-1_3.sdf",
"chars": 829,
"preview": "<?xml version=\"1.0\"?>\n<sdf version=\"1.3\">\n <model name=\"Construction Barrel\">\n <static>true</static>\n <link name="
},
{
"path": "lidarbot_gz/models/construction_barrel/model-1_4.sdf",
"chars": 803,
"preview": "<?xml version=\"1.0\"?>\n<sdf version=\"1.4\">\n <model name=\"Construction Barrel\">\n <link name=\"link\">\n <inertial>\n "
},
{
"path": "lidarbot_gz/models/construction_barrel/model.config",
"chars": 479,
"preview": "<?xml version=\"1.0\"?>\n\n<model>\n <name>Construction Barrel</name>\n <version>1.0</version>\n <sdf version=\"1.3\">model-1_"
},
{
"path": "lidarbot_gz/models/construction_barrel/model.sdf",
"chars": 804,
"preview": "<?xml version=\"1.0\" ?>\n<sdf version=\"1.5\">\n <model name=\"Construction Barrel\">\n <link name=\"link\">\n <inertial>\n"
},
{
"path": "lidarbot_gz/models/construction_cone/meshes/construction_cone.dae",
"chars": 32360,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "lidarbot_gz/models/construction_cone/model-1_3.sdf",
"chars": 631,
"preview": "<?xml version=\"1.0\"?>\n<sdf version=\"1.3\">\n <model name=\"Construction Cone\">\n <static>true</static>\n <link name=\"l"
},
{
"path": "lidarbot_gz/models/construction_cone/model-1_4.sdf",
"chars": 605,
"preview": "<?xml version=\"1.0\"?>\n<sdf version=\"1.4\">\n <model name=\"Construction Cone\">\n <link name=\"link\">\n <collision nam"
},
{
"path": "lidarbot_gz/models/construction_cone/model.config",
"chars": 475,
"preview": "<?xml version=\"1.0\"?>\n\n<model>\n <name>Construction Cone</name>\n <version>1.0</version>\n <sdf version=\"1.3\">model-1_3."
},
{
"path": "lidarbot_gz/models/construction_cone/model.sdf",
"chars": 606,
"preview": "<?xml version=\"1.0\" ?>\n<sdf version=\"1.5\">\n <model name=\"Construction Cone\">\n <link name=\"link\">\n <collision na"
},
{
"path": "lidarbot_gz/package.xml",
"chars": 818,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "lidarbot_gz/urdf/lidarbot_gz.urdf.xacro",
"chars": 1144,
"preview": "<?xml version=\"1.0\" ?>\n<robot name=\"lidarbot\" xmlns:xacro=\"http://www.ros.org/wiki/xacro\">\n\n <xacro:include filename=\"l"
},
{
"path": "lidarbot_gz/urdf/lidarbot_gz_core.xacro",
"chars": 7963,
"preview": "<?xml version=\"1.0\" ?>\n<robot xmlns:xacro=\"http://ros.org/wiki/xacro\">\n\n <xacro:include filename=\"$(find lidarbot_descr"
},
{
"path": "lidarbot_gz/urdf/sensors.xacro",
"chars": 5014,
"preview": "<?xml version=\"1.0\" ?>\n<robot xmlns:xacro=\"http://ros.org/wiki/xacro\">\n\n<!-- Imu sensor -->\n <link name=\"imu_link\">\n "
},
{
"path": "lidarbot_gz/worlds/obstacles.sdf",
"chars": 27041,
"preview": "<?xml version=\"1.0\" ?>\n<sdf version='1.7'>\n <world name='obstacles'>\n\n <physics name=\"1ms\" type='ignored'>\n <ma"
},
{
"path": "lidarbot_navigation/CMakeLists.txt",
"chars": 458,
"preview": "cmake_minimum_required(VERSION 3.8)\nproject(lidarbot_navigation)\n\nfind_package(ament_cmake REQUIRED)\nfind_package(ament_"
},
{
"path": "lidarbot_navigation/config/ekf.yaml",
"chars": 1010,
"preview": "### ekf config file ###\nekf_filter_node:\n ros__parameters:\n\n use_sim_time: true\n \n frequency: 30"
},
{
"path": "lidarbot_navigation/config/nav2_params.yaml",
"chars": 10691,
"preview": "amcl:\n ros__parameters:\n use_sim_time: True\n alpha1: 0.2\n alpha2: 0.2\n alpha3: 0.2\n alpha4: 0.2\n alph"
},
{
"path": "lidarbot_navigation/launch/localization_launch.py",
"chars": 7495,
"preview": "# Copyright (c) 2018 Intel Corporation\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not"
},
{
"path": "lidarbot_navigation/launch/navigation_launch.py",
"chars": 11162,
"preview": "# Copyright (c) 2018 Intel Corporation\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not"
},
{
"path": "lidarbot_navigation/lidarbot_navigation/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "lidarbot_navigation/maps/real_map3.yaml",
"chars": 126,
"preview": "image: real_map3.pgm\nmode: trinary\nresolution: 0.05\norigin: [-3.4, -6.49, 0]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh"
},
{
"path": "lidarbot_navigation/maps/sim_map.yaml",
"chars": 126,
"preview": "image: sim_map.pgm\nmode: trinary\nresolution: 0.05\norigin: [-5.21, -5.44, 0]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh:"
},
{
"path": "lidarbot_navigation/package.xml",
"chars": 797,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "lidarbot_navigation/rviz/lidarbot_nav.rviz",
"chars": 14179,
"preview": "Panels:\n - Class: rviz_common/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:"
},
{
"path": "lidarbot_navigation/scripts/trajectory_visualizer.py",
"chars": 3312,
"preview": "#!/usr/bin/python3\n\n# Node to visualize the robot trajectory in Rviz.\n# Adapted from https://github.com/RBinsonB/traject"
},
{
"path": "lidarbot_slam/CMakeLists.txt",
"chars": 191,
"preview": "cmake_minimum_required(VERSION 3.8)\nproject(lidarbot_slam)\n\nfind_package(ament_cmake REQUIRED)\n\ninstall(\n DIRECTORY con"
},
{
"path": "lidarbot_slam/config/mapper_params_online_async.yaml",
"chars": 2572,
"preview": "slam_toolbox:\n ros__parameters:\n\n # Plugin params\n solver_plugin: solver_plugins::CeresSolver\n ceres_linear_so"
},
{
"path": "lidarbot_slam/launch/online_async_launch.py",
"chars": 2484,
"preview": "import os\n\nfrom launch import LaunchDescription\nfrom launch.actions import DeclareLaunchArgument, LogInfo\nfrom launch.co"
},
{
"path": "lidarbot_slam/package.xml",
"chars": 635,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "lidarbot_slam/rviz/lidarbot_slam.rviz",
"chars": 11326,
"preview": "Panels:\n - Class: rviz_common/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:"
},
{
"path": "lidarbot_teleop/CMakeLists.txt",
"chars": 207,
"preview": "cmake_minimum_required(VERSION 3.8)\nproject(lidarbot_teleop)\n\n# find dependencies\nfind_package(ament_cmake REQUIRED)\n\nin"
},
{
"path": "lidarbot_teleop/config/joystick.yaml",
"chars": 359,
"preview": "joy_node:\n ros__parameters:\n device_id: 0\n deadzone: 0.25\n autorepeat_rate: 20.0 # milliseconds\n\nteleop_node:\n"
},
{
"path": "lidarbot_teleop/config/twist_mux.yaml",
"chars": 218,
"preview": "twist_mux:\n ros__parameters:\n topics:\n navigation:\n topic : cmd_vel\n timeout : 0.5\n prio"
},
{
"path": "lidarbot_teleop/launch/joystick_launch.py",
"chars": 911,
"preview": "# Launch joystick used to drive the robot\n\nimport os\nfrom ament_index_python.packages import get_package_share_directory"
},
{
"path": "lidarbot_teleop/package.xml",
"chars": 696,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
}
]
// ... and 4 more files (download for full content)
About this extraction
This page contains the full source code of the TheNoobInventor/lidarbot GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 131 files (21.9 MB), approximately 5.8M tokens, and a symbol index with 89 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.