├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── joy.config.yaml └── joy_glab.config.yaml ├── docs ├── arduino_example.md ├── images │ ├── amr_control.drawio.png │ ├── amr_scheme.drawio.png │ ├── amr_scheme.drawio.svg │ ├── amr_with_control.drawio.svg │ └── flow.drawio.svg ├── joypad.md ├── launch.md ├── nodes.md ├── parameters.md └── topics.md ├── include ├── ros2_amr_interface │ ├── FIKmodel.hpp │ └── amr_node_class.hpp └── ucPack │ ├── CircularBuffer.cpp │ ├── CircularBuffer.h │ ├── LICENSE │ ├── README.md │ ├── ucPack.cpp │ └── ucPack.h ├── launch ├── __pycache__ │ └── minimal_launch.cpython-38.pyc └── minimal_launch.py ├── package.xml ├── rviz └── test_rviz.rviz └── src └── amr_interface_node.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ros2_amr_interface) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(serial_driver REQUIRED) 22 | find_package(geometry_msgs REQUIRED) 23 | find_package(nav_msgs REQUIRED) 24 | find_package(tf2 REQUIRED) 25 | find_package(tf2_ros REQUIRED) 26 | find_package(sensor_msgs REQUIRED) 27 | 28 | 29 | 30 | add_executable(amr_interface_node src/amr_interface_node.cpp 31 | ./include/ucPack/ucPack.cpp 32 | ./include/ucPack/CircularBuffer.cpp) 33 | 34 | 35 | target_include_directories(amr_interface_node PUBLIC 36 | $ 37 | $) 38 | ament_target_dependencies( 39 | amr_interface_node 40 | "rclcpp" 41 | "serial_driver" 42 | "geometry_msgs" 43 | "nav_msgs" 44 | "tf2" 45 | "tf2_ros" 46 | "sensor_msgs" 47 | ) 48 | 49 | install(TARGETS amr_interface_node 50 | DESTINATION lib/${PROJECT_NAME}) 51 | 52 | install(DIRECTORY launch 53 | DESTINATION share/${PROJECT_NAME}) 54 | 55 | if(BUILD_TESTING) 56 | find_package(ament_lint_auto REQUIRED) 57 | # the following line skips the linter which checks for copyrights 58 | # uncomment the line when a copyright and license is not present in all source files 59 | #set(ament_cmake_copyright_FOUND TRUE) 60 | # the following line skips cpplint (only works in a git repo) 61 | # uncomment the line when this package is not in a git repo 62 | #set(ament_cmake_cpplint_FOUND TRUE) 63 | ament_lint_auto_find_test_dependencies() 64 | endif() 65 | 66 | ament_package() 67 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Giovanni Bruno 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS2 AMR interface 2 | 3 | Lightweight package to get AMRs working with ROS2. 4 | 5 | ## Launch file 6 | 7 | `ros2 launch ros2_amr_interface minimal_launch.py` 8 | 9 | ## Nodes 10 | 11 | - [amr_interface_node](./docs/nodes.md) 12 | 13 | --- 14 | 15 | ## How install 16 | 17 | ### 1. Prerequisites 18 | 19 | - [ROS2](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html) 20 | - [colcon](https://docs.ros.org/en/foxy/Tutorials/Colcon-Tutorial.html) 21 | - [workspace](https://docs.ros.org/en/foxy/Tutorials/Workspace/Creating-A-Workspace.html) 22 | - [permissions on dialout](https://github.com/gbr1/TIL/blob/main/Linux/22-01-24_How_add_user_to_dialout_group.md) 23 | 24 | ### 2. Install AMR interface package 25 | 26 | You require to install dependencies, since binaries sometimes are not updated install transport_drivers from source: 27 | ``` bash 28 | cd ~/dev_ws/src 29 | git clone https://github.com/ros-drivers/transport_drivers.git 30 | cd .. 31 | rosdep install --from-paths src --ignore-src -r -y 32 | colcon build 33 | source install/setup.bash 34 | ``` 35 | 36 | then install amr package by: 37 | ``` bash 38 | cd ~/dev_ws/src 39 | git clone https://github.com/gbr1/ros2_amr_interface.git 40 | cd .. 41 | rosdep install --from-paths src --ignore-src -r -y 42 | colcon build 43 | source install/setup.bash 44 | ``` 45 | 46 | **NOTE:** add install/setup.bash to your .bashrc to be faster in using terminal tabs 47 | 48 |
49 |
50 | 51 | --- 52 | 53 | ## Parameters 54 | 55 | You can learn more about node's parameters [here](./docs/parameters.md). 56 | 57 | ## Topics 58 | 59 | Check this [documentation](./docs/topics.md) about pub/sub topics. 60 | 61 | ## Examples 62 | 63 | If you need to use `ros2_amr_interface` with your own hardware, you can check this [guide](./docs/arduino_example.md) on how to use with arduino. 64 |
65 | There are also some config files for rviz and joypads, here an example of [how to start a joypad](./docs/joypad.md). 66 |
67 | This [guide](./docs/launch.md) helps you to understand how change your launch file parameters. 68 | 69 | --- 70 | 71 | 72 | > ***Copyright (c) 2022 G. Bruno under MIT license*** -------------------------------------------------------------------------------- /config/joy.config.yaml: -------------------------------------------------------------------------------- 1 | teleop_twist_joy_node: 2 | ros__parameters: 3 | axis_linear: 4 | x: 1 5 | y: 0 6 | scale_linear: 7 | x: 0.3 8 | y: 0.3 9 | scale_linear_turbo: 10 | x: 1.0 11 | y: 1.0 12 | 13 | axis_angular: 14 | yaw: 2 15 | scale_angular: 16 | yaw: 1.5 17 | 18 | enable_button: 5 19 | enable_turbo_button: 7 20 | -------------------------------------------------------------------------------- /config/joy_glab.config.yaml: -------------------------------------------------------------------------------- 1 | teleop_twist_joy_node: 2 | ros__parameters: 3 | axis_linear: 4 | x: 1 5 | y: 0 6 | scale_linear: 7 | x: 0.3 8 | y: 0.3 9 | scale_linear_turbo: 10 | x: 1.0 11 | y: 1.0 12 | 13 | axis_angular: 14 | yaw: 3 15 | scale_angular: 16 | yaw: 1.5 17 | 18 | enable_button: 5 19 | enable_turbo_button: 4 -------------------------------------------------------------------------------- /docs/arduino_example.md: -------------------------------------------------------------------------------- 1 | # An Arduino example to use ros2_amr_interface 2 | 3 | Here is provided a guide on how to write your own firmware using Arduino language. 4 | 5 | You need to include [ucPack library](https://github.com/gbr1/ucPack) to our sketch, then to declare an ucPack class. This object will be help you to manage communication to ROS2. 6 | 7 | We need some timers and variables. 8 | 9 | ``` c++ 10 | #include "ucPack.h" 11 | 12 | ucPack packeter(100); 13 | char c; 14 | unsigned long timer_motors = 0; 15 | unsigned long timer_joints = 0; 16 | unsigned long timer_imu = 0; 17 | unsigned long timer_battery = 0; 18 | unsigned long timer_timeout = 0; 19 | 20 | 21 | float accelerometer_x, accelerometer_y, accelerometer_z, gyro_x, gyro_y, gyro_z, temperature; 22 | 23 | float wheel_speed_0, wheel_speed_1, wheel_speed_2, wheel_speed_3; 24 | float w0, w1, w2, w3; 25 | 26 | float battery_voltage = 0.0; 27 | 28 | float timeout = 0.0; 29 | ``` 30 | 31 | In `setup` function, you must start serial communcation and initialize your hardware. Don't forget to save `millis()` in each timer. 32 | 33 | ``` c++ 34 | void setup(){ 35 | Serial.begin(115200); 36 | 37 | // initialize your hardware 38 | 39 | timer_motors = millis(); 40 | timer_joints = millis(); 41 | timer_imu = millis(); 42 | timer_battery = millis(); 43 | timer_timeout = millis(); 44 | } 45 | ``` 46 | 47 | The `loop` function is the core of your firmware. Using the timers we create a polling system to update all the peripherials and get the communication working with ROS2.
48 | The simple idea is to check if any serial data is available and push them into the circular buffer of ucPack. 49 | We need to check if any `payload`, or more simply understandable data, is available.
50 | Using the code you will be able to understand the type of the message and choose the correct operation (e.g. update motors reference). 51 | ``` c++ 52 | void loop(){ 53 | 54 | // update motors speed 55 | if (millis()-timer_motors>10){ 56 | //update motor speed 57 | 58 | timer_motors = millis(); 59 | } 60 | 61 | // send joints data to ros2 62 | if (millis()-timer_joints>10){ 63 | dim = packeter.packetC4F('j', wheel_speed_0, wheel_speed_1, wheel_speed_2, wheel_speed_3); 64 | timer_joints = millis(); 65 | } 66 | 67 | // send imu data to ros2 68 | if (millis()-timer_imu>10){ 69 | //update imu data 70 | 71 | dim = packeter.packetC8F('i', accelerometer_x, accelerometer_y, accelerometer_z, gyro_x, gyro_y, gyro_z, temperature, 0.0); 72 | timer_imu = millis(); 73 | } 74 | 75 | //send battery data to ros2 76 | if (millis()-timer_battery>1000){ 77 | // read battery voltage 78 | 79 | dim = packeter.packetC1F('b', battery_voltage); 80 | Serial.write(packeter.msg, dim); 81 | timer_battery = millis(); 82 | } 83 | 84 | // check timeout 85 | if (millis()-timer_timeout>timeout){ 86 | // your connection was broken, go to a secure operation to block your hardware 87 | } 88 | 89 | // load data from serial port 90 | while(Serial.available>0){ 91 | packeter.buffer.push(Serial.read()); 92 | } 93 | 94 | // unpacket messages from ROS2 95 | while(packeter.checkPayload()){ 96 | timer_timeout = millis(); 97 | c=packeter.payloadTop(); 98 | 99 | // enable robot 100 | if (c=='E'){ 101 | packeter.unpacketC1F(c, timeout); 102 | dim = packeter.packetC1F('e', timeout); 103 | Serial.write(packeter.msg, dim); 104 | 105 | // do other stuffs that make your hardware active 106 | } 107 | 108 | // update joints 109 | if (c=='J'){ 110 | packeter.unpacketC4F(c, w0, w1, w2, w3); 111 | dim = packeter.packetC1F('x', 0.0); 112 | Serial.write(packeter.msg, dim); 113 | 114 | // set motors reference speeds using w0, w1, w2, w3 115 | } 116 | 117 | // stop the robot 118 | if (c=='S'){ 119 | dim = packeter.packetC1F('s', 0.0); 120 | Serial.write(packeter.msg, dim); 121 | // stop the robot 122 | } 123 | 124 | // set gyro scale if imu is connected 125 | if (c=='G'){ 126 | packeter.unpacketC2F(c, accelerometer_scale, gyro_scale); 127 | 128 | // set imu scales and update accelerometer_scale and gyro_scale if they are fully setted 129 | 130 | // acknowledge ros 131 | dim = packeter.packetC2F('g', accelerometer_scale, gyro_scale); 132 | Serial.write(packeter.msg, dim); 133 | } 134 | } 135 | } 136 | ``` 137 | 138 | The timeout is needed if you want to add a security mechanism on hardware fails. If you don't want to use it, you can set `-1` on ROS2 side. 139 | 140 | --- 141 | 142 | 143 | > ***Copyright (c) 2022 G. Bruno under MIT license*** -------------------------------------------------------------------------------- /docs/images/amr_control.drawio.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gbr1/ros2_amr_interface/64dfb5c725034b599d450d61b837e637468c412b/docs/images/amr_control.drawio.png -------------------------------------------------------------------------------- /docs/images/amr_scheme.drawio.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gbr1/ros2_amr_interface/64dfb5c725034b599d450d61b837e637468c412b/docs/images/amr_scheme.drawio.png -------------------------------------------------------------------------------- /docs/images/amr_scheme.drawio.svg: -------------------------------------------------------------------------------- 1 | 7VjLcpswFP0aL50B8V7GdtJuMs1MFo1XHgUJ0BSQK4TB/foKS5hnXDfBaTrJxkH3SlfSOUdHhJmxTMovDG6jO4pwPAMaKmfGagaAZ2vitwrsZcAxdRkIGUEy1Ao8kF9YBdW4MCcIZ52OnNKYk2036NM0xT7vxCBjtOh2C2jcnXULQzwIPPgwHka/E8QjGXWB08S/YhJG9cy67clMAuvOaidZBBEtWiHjZmYsGaVcPiXlEscVdjUuctztM9njwhhO+TkDNn7GisUiX6y/LViR4vnd/XyuquxgnKsNz4Adi3qLgIqyYtV8r6Cwf+a0TsyzA1HXooPubssmKZ7C6i+jGdjAhG1IyjELoI/rumKBsrTsqLA5zgIYzVOEqzXrIl1EhOOHbTXeWBVCYSIW8SRW6YDE8ZLGlB3GGghiN/BFPOOM/sCtjO27+Ck4zrfDjOPyWSD1Iz1C1pgmmLO96KIGGEBRrCQNao0XjUAELjIWtcWhqY5QiTI81m54Ew+Kur+gEYzQeOsnaLOrTmMPYbFz3oWxC1dKU9zDVoVgTMJUNH1csSoCFY5EHJZrlUgIQtU0o7w1zGpKRuq46+40xOiO1iXGswbE6LWxtIkxLsWLMcJLjw2couvKpypYY5hlxO9ycxo1gQzbP6rkobGuGldW3VyV7eRqr1pyGRgNzK+HtXBbyELM/2QhQ05amFsjkNcxhmPIya67jDEe1Az3lBx8SVEO3C7lht6jMqM587Ea1XbHXiHT7BWyeoUkDoNCB1kct/1ypZhjJ5gimnyc42trvePrDH3VfcvTW2upRUoEGSrEjl55bfXA611jgetjf/Qae3It09ImusZMswO3qQ3h1p2Ra8y9GN5jbyPn2GUmDid/jYuWhD82vila69o1xXNjoVWjdtDGeRuzXavZpnFeaV1n6PN9W7Th9ZzVm8qizbe1aH3sNr8lSS5+GSw+sFF7ztXwTcsbkRSwLmUd1hg3T5AL/PYflxkD/Osr1H6hpV/8Dbj2fL3t+FegvgDGTX9K8z7Tu81P757Cu53/S4fvToby8vvU4St1CIb/Ukytw1E9OacFdfnPCNMJ0foU4kkhVm+jx4/KsnvzZd64+Q0= -------------------------------------------------------------------------------- /docs/images/amr_with_control.drawio.svg: -------------------------------------------------------------------------------- 1 | 7Vldb5swFP01eWwV20DIY5u2q/YhTWqlrU+Vgw1YBZw5TkL262eCIXw4TZpBsyl9SfCx77V97/GBCwM0idNPAs/Cb5zQaACHJB2gmwGEY2eofjNgnQMjC+RAIBjJoQrwwH5TDWq7YMEIndcGSs4jyWZ10ONJQj1Zw7AQfFUf5vOoPusMB7QFPHg4aqM/GJFhjrpwtMXvKQvCYmbgjPOeGBeD9U7mISZ8VYHQ7QBNBOcyv4rTCY2y2BVxye3udvSWCxM0kYcYxAm6n319fPl8vYh9y/bRF/l4AXMvSxwt9IYH0ImUv2ufK7dq1XKtQ+H8WvCi42K+SdSVGgDcWbrtVFdB9i/4HD7jWDyzRFLhY48WftUCc9f5QB2bchYo+CIhNFszUN2rkEn6MMvs0c1KMUxhoYwj3e2zKJrwiIuNLSKYur6n8LkU/IVWehzPpVO/nG9JhaTpzkCCMj2K1pTHVIq1GqINUMFpTWk40u3VliCgwMIKOcYaw5qTQel6mzZ1oTP3hiw6rSyGWJCV2tFfhlclSh9J4LbD7bse9Yzhnrq2ZQ/7CTcam8IN2uF2+wr3yHBoGmGmCbnK1Ee1vAjP5ywPExayDVcCvs3O0BB+mjL5M+u7tHXrSY/Mrm/SamNdNBK144pR1nzSs20aW7NNq7DLd0RJSx0baVO75gvh0f3sVJsPqNynRW0aVNJsGw5VgQkaYcmW9eWaUq9n+M7ZRuDSBquaLCtc5NvUVlWZbTiyrIYjq+Eoj0PL0YaJ5baPJ6drIOcdixfqV+BVi6jqXMo6B+uHOeEJbZx8DeGIBUlGZJpJvAKyU87UnfNKd8SMkGwao8q8zvQOZMMZNlTabcvG2MAnaPekGmNTYqZYquCtzzctCMJLu5UY15AY1JecA3Cknh8u3KUIVyW4osg7RLgQfFCV+0tYqL9Z8TtUbnigcrsfyt2BcgPTw3i3PDTyafQ6n44l7wl4OP7gYRc8RAYedlYUqkJd3cmidykHbeoSy1SfuHCKHKef+gTAU5eDwDpSSN5aoOwqLoZ7pMFcyIBXZahDOSkI/n9VJBboSk+cd9YT+zT3tX/+tgbcA3mYE/ZURCyfxguBa/LnUCIie4+jvonYfk92Dg9Yp+KNjUAj3dalfRxznOFeV31zx/TS744THrcodDa1O7DbTzrvW7kbX3Z5MXleZl+hziUv5Sem/vOSvUUsv1jlZ2v72Q/d/gE= -------------------------------------------------------------------------------- /docs/images/flow.drawio.svg: -------------------------------------------------------------------------------- 1 |
g, setted acc scale & gyro scale
amr_interface_node
set reference speeds
J, 4 float
x, float service value
read imu
i, imu data (acc xyz, gyro xyz, 2 float service)
battery
set imu scales
G, acc scale & gyro scale
measure wheels velocities
j, 4 float
b, battery voltage
enable
stop
e, setted timeout
E, timeout
S, service float
s, service float
reply to a message
service float is a float that is not used
 -------------------------------------------------------------------------------- /docs/joypad.md: -------------------------------------------------------------------------------- 1 | ## Use joypad as teleop 2 | 3 | Just launch:
4 | 5 | `ros2 launch teleop_twist_joy teleop-launch.py config_filepath:=/dev_ws/src/ros2_amr_interface/config/joy.config.yaml` 6 | 7 | Configuration files: 8 | - `joy.config.yaml`, for logitech F710 9 | - `joy_glab.config.yaml`, for GLAB remote joypad. 10 | 11 | --- 12 | 13 | 14 | > ***Copyright (c) 2022 G. Bruno under MIT license*** -------------------------------------------------------------------------------- /docs/launch.md: -------------------------------------------------------------------------------- 1 | # Edit launch file 2 | 3 | Here is shown the [minimal launch](../launch/minimal_launch.py): 4 | 5 | ```python 6 | from launch import LaunchDescription 7 | from launch_ros.actions import Node 8 | 9 | def generate_launch_description(): 10 | return LaunchDescription([ 11 | Node( 12 | package='ros2_amr_interface', 13 | executable='amr_interface_node', 14 | name='amr', 15 | remappings=[ 16 | ('/amr/cmd_vel', '/cmd_vel') 17 | ], 18 | parameters=[{ 19 | 'try_reconnect': False 20 | }] 21 | ) 22 | ]) 23 | ``` 24 | 25 | You can edit this file to change your parameters and remaps.
26 | For example, if your need to change the serial port name, you can add in parameters `'serial_port':""`, etc. 27 |
28 |
29 | For all parameters and topics, please check [parameters docs](./parameters.md) and [topics docs](./topics.md). 30 | 31 | --- 32 | 33 | 34 | > ***Copyright (c) 2022 G. Bruno under MIT license*** -------------------------------------------------------------------------------- /docs/nodes.md: -------------------------------------------------------------------------------- 1 | # Nodes 2 | 3 | 4 | ## amr_interface_node 5 | 6 | 7 | This node allows to get serial communication working between hardware and ROS2.
8 | It can manage: 9 | - joints 10 | - odometry 11 | - imu 12 | - battery 13 |
14 | 15 | ![amr_scheme.drawio.png](./images/amr_scheme.drawio.png) 16 |
17 |
18 | 19 | To run:
20 | `ros2 run ros2_amr_interface amr_interface_node` 21 |

22 | if you need to change serial port and remap a topic:
23 | `ros2 run ros2_amr_interface amr_interface_node --ros-args -p port_name:= --remap /amr/cmd_vel:=/cmd_vel` 24 | 25 |
26 | 27 | Here a scheme of communication messages:
28 | ![flow.drawio.svg](./images/flow.drawio.svg) 29 | 30 | --- 31 | 32 | 33 | > ***Copyright (c) 2022 G. Bruno under MIT license*** -------------------------------------------------------------------------------- /docs/parameters.md: -------------------------------------------------------------------------------- 1 | # Parameters 2 | 3 | After running the node, you can check parameters by:
4 | `ros2 param list`
5 | or
6 | `ros2 run rqt_reconfigure rqt_reconfigure` 7 |
8 | 9 | --- 10 | 11 | ### Serial communication 12 | 13 | - `port_name` [ /dev/ttyUSB0 ], _std::string_, port name
14 | - `baud_rate` [ 115200 ], _int_, baud rate
15 | - `disable_timeout` [ true ], _bool_, disable timeout check
16 | - `timeout_connection` [ 60.0 ], _float_, how many seconds of no communication are required to declare timeout, -1.0 disable the timeout
17 | - `try_reconnect` [ false ], _bool_, force reconnection after timeout
18 | - `show_extra_verbose` [ false ], _bool_, show extra verbose in terminal
19 | 20 | 21 | ### IMU 22 | 23 | - `publishIMU` [true], _bool_, enable or disable IMU
24 | - `imu.frame_id` [ imu_link ], _std::string_, frame id used for imu
25 | - `imu.offset.accelerometer.x/y/z` [ 0.0 ], _float_, offset of accelerometer measuraments on x, y and z axis
26 | - `imu.offset.gyro.x/y/z` [0.0], _float_, offset of gyroscope measuraments on x,y and z axis
27 | - `imu.scale_compensation.accelerometer.x/y/z` [ 1.0 ], _float_, compensation accelerometer measuraments on x, y and z axis
28 | - `imu.scale_compensation.gyro.x/y/z` [1.0], _float_, compensation gyroscope measuraments on x,y and z axis
29 | - `imu.scale.accelerometer` [ 2.0 ], _float_, range of m/s^2
30 | - `imu.scale.gyro` [ 250.0 ], _float_, range of rad/s
31 | 32 | 33 | ### Model 34 | 35 | - `publishTF` [ true ], _bool_, broadcast the transformation for odometry
36 | - `frame_id` [ base_link ], _std::string_, robot frame id
37 | - `odom.frame_id` [ odom ], _std::string_, frame id for odometry
38 | - `force_delta_t` [ false ], _bool_, force to use delta_t parameter for odometry integration
39 | - `delta_t` [ 0.01 ], _double_, integration time for odometry
40 | - `model.type` [ mecanum ], _std::string_, options: "mecanum", "differential" and "skid"
41 | - for mecanum:
42 | - `model.size.chassis.x` [ 0.0825 ], _float_, lx on mecanum model (half of the wheel base) 43 | - `model.size.chassis.y` [ 0.105 ], _float_, ly on mecanum model (half of the distance between left wheels and right wheels) 44 | - `model.size.wheel.radius` [ 0.04 ], _float_, wheel radius on mecanum model
45 | - for differential:
46 | - `model.size.chassis.wheel_separation` [0.15], _float_, distance between two wheels 47 | - `model.size.wheel.radius` [0.0325], _float_, wheel radius on differential model 48 | - for skid:
49 | - `model.size.chassis.wheel_separation` [ 0.0825 ], _float_, half of the distance between left wheels and right wheelss 50 | - `model.size.wheel.radius` [ 0.04 ], _float_, wheel radius 51 | 52 | ### Battery 53 | 54 | - `publishBattery` [true], _bool_, enable or disable Battery message
55 | - `battery_max_voltage` [12.5], _float_, maximum voltage of the used battery
56 | - `battery_min_voltage` [10.0], _float_, minimum voltage of the used battery
57 | 58 | 59 | ## Dynamic parameters 60 | - `show_extra_verbose` 61 | - `publish_TF` 62 | - `imu.offset.accelerometer.x/y/z` 63 | - `imu.offset.gyro.x/y/z` 64 | - `imu.scale_compensation.accelerometer.x/y/z` 65 | - `imu.scale_compensation.gyro.x/y/z` 66 | - `imu.scale.accelerometer` 67 | - `imu.scale.gyro` 68 | 69 | --- 70 | 71 | > ***Copyright (c) 2022 G. Bruno under MIT license*** -------------------------------------------------------------------------------- /docs/topics.md: -------------------------------------------------------------------------------- 1 | # Topics 2 | 3 | ## Subscription 4 | - `/amr/cmd_vel`, _geometry_msgs::Twist_, command the robot 5 | - `/amr/initial_pose`, _geometry_msgs::PoseWithCovariance_, set odometry 6 | 7 | ## Published 8 | - `/amr/imu/raw`, _sensor_msgs::Imu_, imu raw data 9 | - `/amr/battery`, _sensor_msgs::BatteryState_, battery voltage 10 | 11 | 12 | --- 13 | 14 | 15 | > ***Copyright (c) 2022 G. Bruno under MIT license*** -------------------------------------------------------------------------------- /include/ros2_amr_interface/FIKmodel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * The MIT License 3 | * 4 | * Copyright (c) 2022 Giovanni di Dio Bruno https://gbr1.github.io 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to deal 8 | * in the Software without restriction, including without limitation the rights 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | * copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | * THE SOFTWARE. 23 | */ 24 | 25 | #ifndef __FIKmodel_HPP__ 26 | #define __FIKmodel_HPP__ 27 | 28 | #include 29 | #include 30 | 31 | class FIKmodel{ 32 | private: 33 | float lx,ly,wheel_radius; 34 | float vx, vy, w; 35 | std::vector joints; 36 | 37 | public: 38 | std::function forward; 39 | std::function inverse; 40 | enum model { MECANUM, DIFFERENTIAL, SKID}; 41 | 42 | FIKmodel(){ 43 | //put something wrong and different by 0.0 44 | this->lx=-1.0; 45 | this->ly=-1.0; 46 | this->wheel_radius=-1.0; 47 | vx=0.0; 48 | vy=0.0; 49 | w=0.0; 50 | }; 51 | 52 | FIKmodel(const model m){ 53 | setModel(m); 54 | vx=0.0; 55 | vy=0.0; 56 | w=0.0; 57 | for (uint8_t i=0; i 89 | 90 | +---+ +---+ 91 | | 0 |-----------| 1 | ^ 92 | +---+ ^ +---+ | 93 | | \ ^ / | | lx 94 | | ^ | | 95 | | O | v 96 | | | 97 | | / \ | 98 | +---+ +---+ 99 | | 2 |-----------| 3 | 100 | +---+ +---+ 101 | 102 | 0: left_front_joint 103 | 1: right_front_joint 104 | 2: left_rear_joint 105 | 3: right_rear_joint 106 | 107 | */ 108 | 109 | void forward_mecanum(){ 110 | joints[0]=(vx-vy-w*(lx+ly))/wheel_radius; 111 | joints[1]=(vx+vy+w*(lx+ly))/wheel_radius; 112 | joints[2]=(vx+vy-w*(lx+ly))/wheel_radius; 113 | joints[3]=(vx-vy+w*(lx+ly))/wheel_radius; 114 | } 115 | 116 | void inverse_mecanum(){ 117 | vx=(joints[0]+joints[1]+joints[2]+joints[3])*wheel_radius/4.0; 118 | vy=(-joints[0]+joints[1]+joints[2]-joints[3])*wheel_radius/4.0; 119 | w=(-joints[0]+joints[1]-joints[2]+joints[3])*wheel_radius/(4.0*(lx+ly)); 120 | } 121 | 122 | //------------------------------------------------------------------------------------// 123 | // DIFFERENTIAL MODEL // 124 | //------------------------------------------------------------------------------------// 125 | 126 | /* 127 | 128 | ly 129 | <---------------> 130 | 131 | +---------------+ 132 | | ^ | 133 | | ^ | 134 | +---+ ^ +---+ 135 | | | | | 136 | | 0 | O | 1 | 137 | | | | | 138 | +---+ +---+ 139 | | | 140 | | | 141 | +---------------+ 142 | 143 | 0: left_joint 144 | 1: right_joint 145 | 146 | */ 147 | 148 | void forward_differential(){ 149 | joints[0]=(2.0*vx-w*ly)/(2.0*wheel_radius); 150 | joints[1]=(2.0*vx+w*ly)/(2.0*wheel_radius); 151 | joints[2]=0.0; 152 | joints[3]=0.0; 153 | } 154 | 155 | void inverse_differential(){ 156 | vx=(joints[0]+joints[1])*wheel_radius/2.0; 157 | vy=0.0; 158 | w=(-joints[0]+joints[1])*wheel_radius/ly; 159 | } 160 | 161 | 162 | 163 | //------------------------------------------------------------------------------------// 164 | // SKID MODEL // 165 | //------------------------------------------------------------------------------------// 166 | 167 | /* 168 | 169 | ly 170 | <-------> 171 | 172 | +---+ +---+ 173 | | 0 |-----------| 1 | ^ 174 | +---+ ^ +---+ | 175 | | ^ | | lx 176 | | ^ | | 177 | | O | v 178 | | | 179 | | | 180 | +---+ +---+ 181 | | 2 |-----------| 3 | 182 | +---+ +---+ 183 | 184 | 0: left_front_joint 185 | 1: right_front_joint 186 | 2: left_rear_joint 187 | 3: right_rear_joint 188 | 189 | */ 190 | 191 | void forward_skid(){ 192 | joints[0]=(vx-ly*w)/wheel_radius; 193 | joints[1]=(vx+ly*w)/wheel_radius; 194 | joints[2]=(vx-ly*w)/wheel_radius; 195 | joints[3]=(vx+ly*w)/wheel_radius; 196 | } 197 | 198 | void inverse_skid(){ 199 | vx=wheel_radius*(joints[0]+joints[1]+joints[2]+joints[3])/4.0; 200 | vy=0.0; //no drift case 201 | w=wheel_radius*(-joints[0]+joints[1]-joints[2]+joints[3])/(4.0*ly); 202 | } 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | //------------------------------------------------------------------------------------// 211 | // Generic interface // 212 | //------------------------------------------------------------------------------------// 213 | 214 | void setDimensions(const float ly, const float r){ 215 | this->lx=0.0; //unused for differential and skid if you don't want to take care about drifting 216 | this->ly=ly; 217 | this->wheel_radius=r; 218 | } 219 | 220 | void setDimensions(const float lx, const float ly, const float r){ 221 | this->lx=lx; 222 | this->ly=ly; 223 | this->wheel_radius=r; 224 | } 225 | 226 | void getJoints(float & w0, float & w1){ 227 | w0=joints[0]; 228 | w1=joints[1]; 229 | } 230 | 231 | void getJoints(float & w0, float & w1, float & w2, float & w3){ 232 | w0=joints[0]; 233 | w1=joints[1]; 234 | w2=joints[2]; 235 | w3=joints[3]; 236 | } 237 | 238 | void setVelocities(const float vel_x, const float vel_w){ 239 | vx=vel_x; 240 | w=vel_w; 241 | } 242 | 243 | void setVelocities(const float vel_x, const float vel_y, const float vel_w){ 244 | vx=vel_x; 245 | vy=vel_y; 246 | w=vel_w; 247 | } 248 | 249 | void setJoints(const float w0, const float & w1){ 250 | joints[0]=w0; 251 | joints[1]=w1; 252 | } 253 | 254 | void setJoints(const float w0, const float w1, const float w2, const float w3){ 255 | joints[0]=w0; 256 | joints[1]=w1; 257 | joints[2]=w2; 258 | joints[3]=w3; 259 | } 260 | 261 | void getVelocities(float & vel_x, float & vel_w){ 262 | vel_x=vx; 263 | vel_w=w; 264 | } 265 | 266 | void getVelocities(float & vel_x, float & vel_y, float & vel_w){ 267 | vel_x=vx; 268 | vel_y=vy; 269 | vel_w=w; 270 | } 271 | 272 | }; 273 | 274 | 275 | #endif /* FIKmodel_h */ 276 | -------------------------------------------------------------------------------- /include/ros2_amr_interface/amr_node_class.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * The MIT License 3 | * 4 | * Copyright (c) 2022 Giovanni di Dio Bruno https://gbr1.github.io 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to deal 8 | * in the Software without restriction, including without limitation the rights 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | * copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | * THE SOFTWARE. 23 | */ 24 | #ifndef __AMR_NODE_CLASS_HPP__ 25 | #define __AMR_NODE_CLASS_HPP__ 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "rclcpp/rclcpp.hpp" 35 | #include "serial_driver/serial_driver.hpp" 36 | 37 | #include "geometry_msgs/msg/twist.hpp" 38 | #include "nav_msgs/msg/odometry.hpp" 39 | #include "tf2/LinearMath/Quaternion.h" 40 | #include "tf2_ros/transform_broadcaster.h" 41 | #include "tf2/LinearMath/Matrix3x3.h" 42 | #include "sensor_msgs/msg/imu.hpp" 43 | #include "sensor_msgs/msg/battery_state.hpp" 44 | #include "rcl_interfaces/msg/set_parameters_result.hpp" 45 | 46 | #include "ros2_amr_interface/FIKmodel.hpp" 47 | #include "ucPack/ucPack.h" 48 | 49 | using namespace std::chrono_literals; 50 | 51 | 52 | 53 | class AMR_Node: public rclcpp::Node{ 54 | private: 55 | FIKmodel fik_model; 56 | float w_1,w_2,w_3,w_4; 57 | std::string model; 58 | float model_lx, model_ly, model_wheel; 59 | double imu_offset_acc_x, imu_offset_acc_y, imu_offset_acc_z, imu_offset_gyro_x, imu_offset_gyro_y, imu_offset_gyro_z, acc_scale, gyro_scale; 60 | double imu_compensation_acc_x, imu_compensation_acc_y, imu_compensation_acc_z, imu_compensation_gyro_x, imu_compensation_gyro_y, imu_compensation_gyro_z; 61 | float vx, vy, w, x, y, theta, ax, ay, az, gx, gy, gz; 62 | double dt; 63 | double delta_t; 64 | bool force_delta_t; 65 | float dtheta, dx, dy; 66 | float battery, battery_max_voltage, battery_min_voltage, battery_percentage; 67 | bool publishTF; 68 | bool publishImu; 69 | bool publishBattery; 70 | float timeout_connection; 71 | bool connected; 72 | bool try_reconnect; 73 | bool disable_timeout; 74 | 75 | bool joint_data_available; 76 | bool to_be_publish; 77 | bool imu_data_available; 78 | bool battery_data_available; 79 | 80 | bool extra_verbose; 81 | 82 | std::string imu_link, odom_link, robot_link; 83 | 84 | // Required for serial communication 85 | std::unique_ptr node_ctx{}; 86 | std::string device_name; 87 | int baud_rate; 88 | std::unique_ptr device_config; 89 | std::unique_ptr serial_driver{}; 90 | 91 | ucPack packeter; 92 | 93 | 94 | // Timers and times 95 | rclcpp::Time previous_time; 96 | rclcpp::TimerBase::SharedPtr odom_timer; 97 | rclcpp::TimerBase::SharedPtr imu_timer; 98 | rclcpp::TimerBase::SharedPtr battery_timer; 99 | rclcpp::TimerBase::SharedPtr connection_timer; 100 | rclcpp::TimerBase::SharedPtr send_joint_timer; 101 | 102 | rclcpp::Time timeout_time; 103 | 104 | 105 | rclcpp::TimerBase::SharedPtr send_timer; 106 | 107 | // Subscribers and Publishers 108 | rclcpp::Subscription::SharedPtr cmd_subscription; 109 | rclcpp::Subscription::SharedPtr initial_pose_subscription; 110 | rclcpp::Publisher::SharedPtr odom_publisher; 111 | rclcpp::Publisher::SharedPtr imu_publisher; 112 | std::unique_ptr tf_bc; 113 | rclcpp::Publisher::SharedPtr battery_publisher; 114 | 115 | OnSetParametersCallbackHandle::SharedPtr parameters_callback_handle; 116 | 117 | std::vector serial_msg; 118 | uint8_t dim; 119 | bool cmd_is_exec; 120 | bool closing_is_exec; 121 | bool ask_to_close; 122 | 123 | 124 | // Callback for /cmd_vel topic subscription 125 | void cmd_callback(const geometry_msgs::msg::Twist::SharedPtr msg){ 126 | fik_model.setVelocities(msg->linear.x,msg->linear.y,msg->angular.z); 127 | fik_model.forward(); 128 | fik_model.getJoints(w_1,w_2,w_3,w_4); 129 | 130 | /* 131 | fik_model.getJoints(w1,w2,w3,w4); 132 | dim=packeter.packetC4F('J',w1,w2,w3,w4); 133 | for(uint8_t i=0; iport()->async_send(serial_msg); 138 | send_timer = this->create_wall_timer(1ms, std::bind(&AMR_Node::send_callback, this)); 139 | 140 | //-------------------------------------------------------------------- 141 | if (extra_verbose){ 142 | RCLCPP_INFO(this->get_logger(),"sent: %f\t%f\t%f\t%f",w1,w2,w3,w4); 143 | } 144 | //-------------------------------------------------------------------- 145 | 146 | */ 147 | } 148 | 149 | // Callback every 10ms to send data via serial to update joints, this begins afert 'e' was decoded 150 | void send_joints_callback(){ 151 | uint8_t dim=packeter.packetC4F('J',w_1,w_2,w_3,w_4); 152 | for(uint8_t i=0; iport()->async_send(serial_msg); 157 | send_timer = this->create_wall_timer(1ms, std::bind(&AMR_Node::send_callback, this)); 158 | 159 | //-------------------------------------------------------------------- 160 | if (extra_verbose){ 161 | RCLCPP_INFO(this->get_logger(),"sent: %f\t%f\t%f\t%f",w_1,w_2,w_3,w_4); 162 | } 163 | //-------------------------------------------------------------------- 164 | } 165 | 166 | 167 | // Callback to resend joint message until ack from hardware 168 | void send_callback(){ 169 | if (!cmd_is_exec){ 170 | serial_driver->port()->async_send(serial_msg); 171 | } 172 | else{ 173 | serial_msg.clear(); 174 | send_timer->cancel(); 175 | } 176 | } 177 | 178 | // Callback for /initial_pose topic subscription 179 | void initial_pose_callback(const geometry_msgs::msg::PoseWithCovariance::SharedPtr msg) { 180 | x=msg->pose.position.x; 181 | y=msg->pose.position.y; 182 | 183 | tf2::Quaternion q(msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w); 184 | tf2::Matrix3x3 m(q); 185 | double roll,pitch,yaw; 186 | m.getRPY(roll,pitch,yaw); 187 | theta=float(yaw); 188 | //--------------------------------------------------------------------------------------------- 189 | if (extra_verbose){ 190 | RCLCPP_INFO(this->get_logger(), "new pose:\t\t%f %f %f\t%f %f %f", x, y, 0.0, 0.0, 0.0, theta); 191 | } 192 | //--------------------------------------------------------------------------------------------- 193 | } 194 | 195 | // Callback for serial communication 196 | void serial_callback(const std::vector & buffer, const size_t & bytes_transferred){ 197 | for (int i=0; iget_clock()->now(); 205 | } 206 | if (!connected){ 207 | if (c=='e'){ 208 | if ((timeout_connection<=0.0)||(disable_timeout)){ 209 | connection_timer->cancel(); 210 | } 211 | connected=true; 212 | RCLCPP_INFO(this->get_logger(),"Hardware is now online"); 213 | //Set imu scales 214 | setImuScales(acc_scale,gyro_scale); 215 | send_joint_timer = this->create_wall_timer(10ms, std::bind(&AMR_Node::send_joints_callback, this)); 216 | 217 | } 218 | } 219 | else{ 220 | // joints message from hardware 221 | if (c=='j'){ 222 | float w1,w2,w3,w4; 223 | packeter.unpacketC4F(c,w1,w2,w3,w4); 224 | rclcpp::Time now = this->get_clock()->now(); 225 | //--------------------------------------------------------------------------- 226 | if (extra_verbose){ 227 | RCLCPP_INFO(this->get_logger(),"joints: %f\t%f\t%f\t%f",w1,w2,w3,w4); 228 | } 229 | //--------------------------------------------------------------------------- 230 | fik_model.setJoints(w1,w2,w3,w4); 231 | fik_model.inverse(); 232 | fik_model.getVelocities(vx,vy,w); 233 | if (force_delta_t){ 234 | dt=delta_t; 235 | } 236 | else{ 237 | dt=now.seconds()-previous_time.seconds(); 238 | } 239 | previous_time=now; 240 | 241 | dtheta=w*dt; 242 | //theta+=dtheta; 243 | dx=(vx*cos(theta)-vy*sin(theta))*dt; 244 | dy=(vx*sin(theta)+vy*cos(theta))*dt; 245 | x+=dx; 246 | y+=dy; 247 | theta+=dtheta; 248 | joint_data_available=true; 249 | } else 250 | 251 | // imu message from hardware 252 | if ((c=='i')&&publishImu){ 253 | float temp, f; 254 | packeter.unpacketC8F(c,ax,ay,az,gx,gy,gz,temp,f); 255 | //----------------------------------------------------------------------------------------- 256 | if (extra_verbose){ 257 | RCLCPP_INFO(this->get_logger(),"imu: %f\t%f\t%f\t%f\t%f\t%f\t%f",ax,ay,az,gx,gy,gz,temp); 258 | } 259 | //----------------------------------------------------------------------------------------- 260 | imu_data_available=true; 261 | } else 262 | 263 | // battery message from hardware 264 | if ((c=='b')&&publishBattery){ 265 | packeter.unpacketC1F(c,battery); 266 | //----------------------------------------------------------------------------------------- 267 | if (extra_verbose){ 268 | RCLCPP_INFO(this->get_logger(),"battery: %f V",battery); 269 | } 270 | 271 | //----------------------------------------------------------------------------------------- 272 | battery_data_available=true; 273 | } else 274 | 275 | if (c=='s'){ 276 | send_joint_timer->cancel(); 277 | closing_is_exec=true; 278 | float f; 279 | packeter.unpacketC1F(c,f); 280 | //----------------------------------------------------------------------------------------- 281 | RCLCPP_WARN(this->get_logger(),"Hardware is stopped"); 282 | //----------------------------------------------------------------------------------------- 283 | if (try_reconnect&&!ask_to_close){ 284 | connected=false; 285 | RCLCPP_WARN(this->get_logger(),"Try reconnecting to the hardware. If this message is repeated, check your hardware"); 286 | } 287 | else{ 288 | rclcpp::shutdown(); 289 | } 290 | } else 291 | 292 | if ((c=='g')&&publishImu){ 293 | float a,g; 294 | packeter.unpacketC2F(c,a,g); 295 | if (a<=0){ 296 | RCLCPP_ERROR(this->get_logger(),"Wrong parameter on accelerometer scale: %f m/s^2", acc_scale); 297 | } 298 | if (g<=0){ 299 | RCLCPP_ERROR(this->get_logger(),"Wrong parameter on gyro scale: %f rad/s", gyro_scale); 300 | } 301 | } else 302 | 303 | // hardware received joint command 304 | if (c=='x'){ 305 | cmd_is_exec=true; 306 | } 307 | } 308 | 309 | } 310 | } 311 | 312 | // Odometry publisher and TF broadcaster 313 | void odom_pub_callback(){ 314 | if (joint_data_available){ 315 | rclcpp::Time now = this->get_clock()->now(); 316 | tf2::Quaternion q; 317 | q.setRPY(0.0,0.0,this->theta); 318 | 319 | if (publishTF){ 320 | geometry_msgs::msg::TransformStamped t; 321 | 322 | t.header.stamp = now; 323 | t.header.frame_id = odom_link; 324 | t.child_frame_id = robot_link; 325 | 326 | t.transform.translation.x = this->x; 327 | t.transform.translation.y = this->y; 328 | t.transform.translation.z = 0.0; 329 | 330 | 331 | t.transform.rotation.x = q.x(); 332 | t.transform.rotation.y = q.y(); 333 | t.transform.rotation.z = q.z(); 334 | t.transform.rotation.w = q.w(); 335 | 336 | this->tf_bc->sendTransform(t); 337 | } 338 | 339 | 340 | nav_msgs::msg::Odometry odom; 341 | odom.header.stamp=now; 342 | odom.header.frame_id=odom_link; 343 | odom.child_frame_id=robot_link; 344 | odom.pose.pose.position.x=this->x; 345 | odom.pose.pose.position.y=this->y; 346 | odom.pose.pose.position.z=0.0; 347 | odom.pose.pose.orientation.x=q.x(); 348 | odom.pose.pose.orientation.y=q.y(); 349 | odom.pose.pose.orientation.z=q.z(); 350 | odom.pose.pose.orientation.w=q.w(); 351 | odom.child_frame_id=robot_link; 352 | odom.twist.twist.linear.x = this->vx; 353 | odom.twist.twist.linear.y = this->vy; 354 | odom.twist.twist.linear.z = 0.0; 355 | odom.twist.twist.angular.x=0.0; 356 | odom.twist.twist.angular.y=0.0; 357 | odom.twist.twist.angular.z=this->w; 358 | 359 | odom_publisher->publish(odom); 360 | /* 361 | vx=0.0; 362 | vy=0.0; 363 | w=0.0; 364 | */ 365 | joint_data_available=false; 366 | } 367 | 368 | } 369 | 370 | // Imu publisher 371 | void imu_pub_callback(){ 372 | if (imu_data_available){ 373 | rclcpp::Time now = this->get_clock()->now(); 374 | sensor_msgs::msg::Imu imu; 375 | imu.header.stamp=now; 376 | imu.header.frame_id=imu_link; 377 | imu.linear_acceleration.x=imu_compensation_acc_x*(ax+imu_offset_acc_x); 378 | imu.linear_acceleration.y=imu_compensation_acc_y*(ay+imu_offset_acc_y); 379 | imu.linear_acceleration.z=imu_compensation_acc_z*(az+imu_offset_acc_z); 380 | /* 381 | tf2::Quaternion q; 382 | q.setRPY(gx,gy,gz); 383 | imu.orientation.x=q.x(); 384 | imu.orientation.y=q.y(); 385 | imu.orientation.z=q.z(); 386 | imu.orientation.w=q.w(); 387 | */ 388 | imu.orientation_covariance[0]=-1; 389 | imu.angular_velocity.x=imu_compensation_gyro_x*(gx+imu_offset_gyro_x); 390 | imu.angular_velocity.y=imu_compensation_gyro_y*(gy+imu_offset_gyro_y); 391 | imu.angular_velocity.z=imu_compensation_gyro_z*(gz+imu_offset_gyro_z); 392 | 393 | imu_publisher->publish(imu); 394 | imu_data_available = false; 395 | } 396 | } 397 | 398 | // Battery publisher 399 | void battery_pub_callback(){ 400 | if (battery_data_available){ 401 | battery_percentage=100.0*((battery-battery_min_voltage)/(battery_max_voltage-battery_min_voltage)); 402 | if (battery_percentage>100.0){ 403 | battery_percentage=100.0; 404 | } 405 | if (battery_percentage<0.0){ 406 | battery_percentage=0.0; 407 | } 408 | rclcpp::Time now = this->get_clock()->now(); 409 | sensor_msgs::msg::BatteryState battery_msg; 410 | battery_msg.header.stamp=now; 411 | battery_msg.header.frame_id="base_link"; 412 | battery_msg.voltage=battery; 413 | battery_msg.percentage=battery_percentage; 414 | battery_publisher->publish(battery_msg); 415 | battery_data_available=false; 416 | } 417 | } 418 | 419 | // This callback is used for dynamics parameters 420 | rcl_interfaces::msg::SetParametersResult parameters_callback(const std::vector ¶meters){ 421 | rcl_interfaces::msg::SetParametersResult result; 422 | result.successful = false; 423 | for (const auto & param : parameters){ 424 | 425 | if (param.get_name() == "show_extra_verbose"){ 426 | rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_BOOL; 427 | if (param.get_type() != correctType){ 428 | result.successful = false; 429 | result.reason = param.get_name()+" setted as "+rclcpp::to_string(param.get_type())+" but declared as "+rclcpp::to_string(correctType); 430 | RCLCPP_WARN_STREAM(get_logger(),result.reason); 431 | return result; 432 | } 433 | extra_verbose = param.as_bool(); 434 | result.successful=true; 435 | result.reason="Parameter "+param.get_name()+" setted correctly!"; 436 | return result; 437 | } 438 | 439 | if (param.get_name() == "try_reconnect"){ 440 | rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_BOOL; 441 | if (param.get_type() != correctType){ 442 | result.successful = false; 443 | result.reason = param.get_name()+" setted as "+rclcpp::to_string(param.get_type())+" but declared as "+rclcpp::to_string(correctType); 444 | RCLCPP_WARN_STREAM(get_logger(),result.reason); 445 | return result; 446 | } 447 | try_reconnect = param.as_bool(); 448 | result.successful=true; 449 | result.reason="Parameter "+param.get_name()+" setted correctly!"; 450 | return result; 451 | } 452 | 453 | if (param.get_name() == "publishTF"){ 454 | rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_BOOL; 455 | if (param.get_type() != correctType){ 456 | result.successful = false; 457 | result.reason = param.get_name()+" setted as "+rclcpp::to_string(param.get_type())+" but declared as "+rclcpp::to_string(correctType); 458 | RCLCPP_WARN_STREAM(get_logger(),result.reason); 459 | return result; 460 | } 461 | publishTF = param.as_bool(); 462 | result.successful=true; 463 | result.reason="Parameter "+param.get_name()+" setted correctly!"; 464 | return result; 465 | } 466 | 467 | 468 | if (publishImu&&(param.get_name() == "imu.offsets.accelerometer.x")){ 469 | rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_DOUBLE; 470 | if (param.get_type() != correctType){ 471 | result.successful = false; 472 | result.reason = param.get_name()+" setted as "+rclcpp::to_string(param.get_type())+" but declared as "+rclcpp::to_string(correctType); 473 | RCLCPP_WARN_STREAM(get_logger(),result.reason); 474 | return result; 475 | } 476 | imu_offset_acc_x = param.as_double(); 477 | result.successful=true; 478 | result.reason="Parameter "+param.get_name()+" setted correctly!"; 479 | return result; 480 | } 481 | 482 | if (publishImu&&(param.get_name() == "imu.offsets.accelerometer.y")){ 483 | rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_DOUBLE; 484 | if (param.get_type() != correctType){ 485 | result.successful = false; 486 | result.reason = param.get_name()+" setted as "+rclcpp::to_string(param.get_type())+" but declared as "+rclcpp::to_string(correctType); 487 | RCLCPP_WARN_STREAM(get_logger(),result.reason); 488 | return result; 489 | } 490 | imu_offset_acc_y = param.as_double(); 491 | result.successful=true; 492 | result.reason="Parameter "+param.get_name()+" setted correctly!"; 493 | return result; 494 | } 495 | 496 | if (publishImu&&(param.get_name() == "imu.offsets.accelerometer.z")){ 497 | rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_DOUBLE; 498 | if (param.get_type() != correctType){ 499 | result.successful = false; 500 | result.reason = param.get_name()+" setted as "+rclcpp::to_string(param.get_type())+" but declared as "+rclcpp::to_string(correctType); 501 | RCLCPP_WARN_STREAM(get_logger(),result.reason); 502 | return result; 503 | } 504 | imu_offset_acc_z = param.as_double(); 505 | result.successful=true; 506 | result.reason="Parameter "+param.get_name()+" setted correctly!"; 507 | return result; 508 | } 509 | 510 | if (publishImu&&(param.get_name() == "imu.offsets.gyro.x")){ 511 | rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_DOUBLE; 512 | if (param.get_type() != correctType){ 513 | result.successful = false; 514 | result.reason = param.get_name()+" setted as "+rclcpp::to_string(param.get_type())+" but declared as "+rclcpp::to_string(correctType); 515 | RCLCPP_WARN_STREAM(get_logger(),result.reason); 516 | return result; 517 | } 518 | imu_offset_gyro_x = param.as_double(); 519 | result.successful=true; 520 | result.reason="Parameter "+param.get_name()+" setted correctly!"; 521 | return result; 522 | } 523 | 524 | if (publishImu&&(param.get_name() == "imu.offsets.gyro.y")){ 525 | rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_DOUBLE; 526 | if (param.get_type() != correctType){ 527 | result.successful = false; 528 | result.reason = param.get_name()+" setted as "+rclcpp::to_string(param.get_type())+" but declared as "+rclcpp::to_string(correctType); 529 | RCLCPP_WARN_STREAM(get_logger(),result.reason); 530 | return result; 531 | } 532 | imu_offset_gyro_y = param.as_double(); 533 | result.successful=true; 534 | result.reason="Parameter "+param.get_name()+" setted correctly!"; 535 | return result; 536 | } 537 | 538 | if (publishImu&&(param.get_name() == "imu.offsets.gyro.z")){ 539 | rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_DOUBLE; 540 | if (param.get_type() != correctType){ 541 | result.successful = false; 542 | result.reason = param.get_name()+" setted as "+rclcpp::to_string(param.get_type())+" but declared as "+rclcpp::to_string(correctType); 543 | RCLCPP_WARN_STREAM(get_logger(),result.reason); 544 | return result; 545 | } 546 | imu_offset_gyro_z = param.as_double(); 547 | result.successful=true; 548 | result.reason="Parameter "+param.get_name()+" setted correctly!"; 549 | return result; 550 | } 551 | 552 | if (publishImu&&(param.get_name() == "imu.scale_compensation.accelerometer.x")){ 553 | rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_DOUBLE; 554 | if (param.get_type() != correctType){ 555 | result.successful = false; 556 | result.reason = param.get_name()+" setted as "+rclcpp::to_string(param.get_type())+" but declared as "+rclcpp::to_string(correctType); 557 | RCLCPP_WARN_STREAM(get_logger(),result.reason); 558 | return result; 559 | } 560 | imu_compensation_acc_x = param.as_double(); 561 | result.successful=true; 562 | result.reason="Parameter "+param.get_name()+" setted correctly!"; 563 | return result; 564 | } 565 | 566 | if (publishImu&&(param.get_name() == "imu.scale_compensation.accelerometer.y")){ 567 | rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_DOUBLE; 568 | if (param.get_type() != correctType){ 569 | result.successful = false; 570 | result.reason = param.get_name()+" setted as "+rclcpp::to_string(param.get_type())+" but declared as "+rclcpp::to_string(correctType); 571 | RCLCPP_WARN_STREAM(get_logger(),result.reason); 572 | return result; 573 | } 574 | imu_compensation_acc_y = param.as_double(); 575 | result.successful=true; 576 | result.reason="Parameter "+param.get_name()+" setted correctly!"; 577 | return result; 578 | } 579 | 580 | if (publishImu&&(param.get_name() == "imu.scale_compensation.accelerometer.z")){ 581 | rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_DOUBLE; 582 | if (param.get_type() != correctType){ 583 | result.successful = false; 584 | result.reason = param.get_name()+" setted as "+rclcpp::to_string(param.get_type())+" but declared as "+rclcpp::to_string(correctType); 585 | RCLCPP_WARN_STREAM(get_logger(),result.reason); 586 | return result; 587 | } 588 | imu_compensation_acc_z = param.as_double(); 589 | result.successful=true; 590 | result.reason="Parameter "+param.get_name()+" setted correctly!"; 591 | return result; 592 | } 593 | 594 | if (publishImu&&(param.get_name() == "imu.scale_compensation.gyro.x")){ 595 | rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_DOUBLE; 596 | if (param.get_type() != correctType){ 597 | result.successful = false; 598 | result.reason = param.get_name()+" setted as "+rclcpp::to_string(param.get_type())+" but declared as "+rclcpp::to_string(correctType); 599 | RCLCPP_WARN_STREAM(get_logger(),result.reason); 600 | return result; 601 | } 602 | imu_compensation_gyro_x = param.as_double(); 603 | result.successful=true; 604 | result.reason="Parameter "+param.get_name()+" setted correctly!"; 605 | return result; 606 | } 607 | 608 | if (publishImu&&(param.get_name() == "imu.scale_compensation.gyro.y")){ 609 | rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_DOUBLE; 610 | if (param.get_type() != correctType){ 611 | result.successful = false; 612 | result.reason = param.get_name()+" setted as "+rclcpp::to_string(param.get_type())+" but declared as "+rclcpp::to_string(correctType); 613 | RCLCPP_WARN_STREAM(get_logger(),result.reason); 614 | return result; 615 | } 616 | imu_compensation_gyro_y = param.as_double(); 617 | result.successful=true; 618 | result.reason="Parameter "+param.get_name()+" setted correctly!"; 619 | return result; 620 | } 621 | 622 | if (publishImu&&(param.get_name() == "imu.scale_compensation.gyro.z")){ 623 | rclcpp::ParameterType correctType = rclcpp::ParameterType::PARAMETER_DOUBLE; 624 | if (param.get_type() != correctType){ 625 | result.successful = false; 626 | result.reason = param.get_name()+" setted as "+rclcpp::to_string(param.get_type())+" but declared as "+rclcpp::to_string(correctType); 627 | RCLCPP_WARN_STREAM(get_logger(),result.reason); 628 | return result; 629 | } 630 | imu_compensation_gyro_z = param.as_double(); 631 | result.successful=true; 632 | result.reason="Parameter "+param.get_name()+" setted correctly!"; 633 | return result; 634 | } 635 | } 636 | return result; 637 | } 638 | 639 | void check_connection(){ 640 | if (!connected){ 641 | std::vector serial_msg; 642 | uint8_t dim=packeter.packetC1F('E',timeout_connection); 643 | for(uint8_t i=0; iport()->async_send(serial_msg); 647 | serial_msg.clear(); 648 | } 649 | else{ 650 | if (disable_timeout){ 651 | connection_timer->cancel(); 652 | } 653 | else{ 654 | if ((this->get_clock()->now().seconds()-timeout_time.seconds())>timeout_connection){ 655 | RCLCPP_WARN(this->get_logger(),"serial connection is timed out, no message in about %f seconds despite %f setted", this->get_clock()->now().seconds()-timeout_time.seconds(),timeout_connection); 656 | RCLCPP_WARN(this->get_logger(),"Trying to restart hardware"); 657 | connected=false; 658 | } 659 | } 660 | } 661 | 662 | } 663 | 664 | // Here are declared all parameters 665 | void parameters_declaration(){ 666 | this->declare_parameter("port_name","/dev/ttyUSB0"); 667 | this->declare_parameter("baud_rate",115200); 668 | this->declare_parameter("disable_timeout",true); 669 | this->declare_parameter("timeout_connection",60.0); 670 | this->declare_parameter("try_reconnect",false); 671 | this->declare_parameter("publishTF",true); 672 | 673 | this->declare_parameter("odom.frame_id","odom"); 674 | this->declare_parameter("frame_id","base_link"); 675 | 676 | this->declare_parameter("force_delta_t",false); 677 | this->declare_parameter("delta_t",0.01); 678 | 679 | this->declare_parameter("publishBattery",true); 680 | this->declare_parameter("battery_max_voltage",12.5); 681 | this->declare_parameter("battery_min_voltage",10.0); 682 | 683 | 684 | this->declare_parameter("show_extra_verbose", false); 685 | } 686 | 687 | // Load static parameters 688 | void get_all_parameters(){ 689 | this->get_parameter("port_name",device_name); 690 | this->get_parameter("baud_rate",baud_rate); 691 | this->get_parameter("disable_timeout",disable_timeout); 692 | this->get_parameter("timeout_connection",timeout_connection); 693 | this->get_parameter("try_reconnect",try_reconnect); 694 | this->get_parameter("publishTF",publishTF); 695 | 696 | this->get_parameter("odom.frame_id",odom_link); 697 | this->get_parameter("frame_id",robot_link); 698 | 699 | this->get_parameter("force_delta_t",force_delta_t); 700 | this->get_parameter("delta_t",delta_t); 701 | 702 | this->get_parameter("publishBattery", publishBattery); 703 | this->get_parameter("battery_max_voltage", battery_max_voltage); 704 | this->get_parameter("battery_min_voltage", battery_min_voltage); 705 | 706 | 707 | this->get_parameter("show_extra_verbose", extra_verbose); 708 | 709 | } 710 | 711 | // Math model parameters 712 | void model_parameters(){ 713 | 714 | this->declare_parameter("model.type","mecanum"); 715 | this->get_parameter("model.type",model); 716 | 717 | if (model.compare("mecanum")==0){ 718 | this->declare_parameter("model.size.chassis.x",0.0825); 719 | this->declare_parameter("model.size.chassis.y",0.105); 720 | this->declare_parameter("model.size.wheel.radius",0.04); 721 | 722 | this->get_parameter("model.size.chassis.x",model_lx); 723 | this->get_parameter("model.size.chassis.y",model_ly); 724 | this->get_parameter("model.size.wheel.radius",model_wheel); 725 | 726 | fik_model.setModel(FIKmodel::model::MECANUM); 727 | fik_model.setDimensions(model_lx, model_ly, model_wheel); 728 | } else 729 | 730 | if (model.compare("differential")==0){ 731 | this->declare_parameter("model.size.chassis.wheel_separation",0.15); 732 | this->declare_parameter("model.size.wheel.radius",0.0325); 733 | 734 | fik_model.setModel(FIKmodel::model::DIFFERENTIAL); 735 | this->get_parameter("model.size.chassis.wheel_separation",model_ly); 736 | this->get_parameter("model.size.wheel.radius",model_wheel); 737 | fik_model.setDimensions(model_ly, model_wheel); 738 | } else 739 | 740 | if (model.compare("skid")==0){ 741 | this->declare_parameter("model.size.chassis.wheel_separation",0.0825); 742 | this->declare_parameter("model.size.wheel.radius",0.105); 743 | 744 | fik_model.setModel(FIKmodel::model::SKID); 745 | this->get_parameter("model.size.chassis.wheel_separation",model_ly); 746 | this->get_parameter("model.size.wheel.radius",model_wheel); 747 | fik_model.setDimensions(model_ly, model_wheel); 748 | } else { 749 | RCLCPP_ERROR(this->get_logger(),"wrong paramenter on model.type, it can't be %s", model.c_str()); 750 | this->~AMR_Node(); 751 | } 752 | 753 | 754 | } 755 | 756 | // IMU parameters 757 | void imu_parameters(){ 758 | this->declare_parameter("publishIMU",true); 759 | this->get_parameter("publishIMU", publishImu); 760 | 761 | if (publishImu){ 762 | this->declare_parameter("imu.frame_id","imu_link"); 763 | this->declare_parameter("imu.offsets.accelerometer.x",0.0); 764 | this->declare_parameter("imu.offsets.accelerometer.y",0.0); 765 | this->declare_parameter("imu.offsets.accelerometer.z",0.0); 766 | this->declare_parameter("imu.offsets.gyro.x",0.0); 767 | this->declare_parameter("imu.offsets.gyro.y",0.0); 768 | this->declare_parameter("imu.offsets.gyro.z",0.0); 769 | this->declare_parameter("imu.scale_compensation.accelerometer.x",1.0); 770 | this->declare_parameter("imu.scale_compensation.accelerometer.y",1.0); 771 | this->declare_parameter("imu.scale_compensation.accelerometer.z",1.0); 772 | this->declare_parameter("imu.scale_compensation.gyro.x",1.0); 773 | this->declare_parameter("imu.scale_compensation.gyro.y",1.0); 774 | this->declare_parameter("imu.scale_compensation.gyro.z",1.0); 775 | this->declare_parameter("imu.scale.accelerometer",2.0); 776 | this->declare_parameter("imu.scale.gyro",250.0); 777 | 778 | this->get_parameter("imu.frame_id",imu_link); 779 | this->get_parameter("imu.offsets.accelerometer.x",imu_offset_acc_x); 780 | this->get_parameter("imu.offsets.accelerometer.y",imu_offset_acc_y); 781 | this->get_parameter("imu.offsets.accelerometer.z",imu_offset_acc_z); 782 | this->get_parameter("imu.offsets.gyro.x",imu_offset_gyro_x); 783 | this->get_parameter("imu.offsets.gyro.y",imu_offset_gyro_y); 784 | this->get_parameter("imu.offsets.gyro.z",imu_offset_gyro_z); 785 | this->get_parameter("imu.scale_compensation.accelerometer.x",imu_compensation_acc_x); 786 | this->get_parameter("imu.scale_compensation.accelerometer.y",imu_compensation_acc_y); 787 | this->get_parameter("imu.scale_compensation.accelerometer.z",imu_compensation_acc_z); 788 | this->get_parameter("imu.scale_compensation.gyro.x",imu_compensation_gyro_x); 789 | this->get_parameter("imu.scale_compensation.gyro.y",imu_compensation_gyro_y); 790 | this->get_parameter("imu.scale_compensation.gyro.z",imu_compensation_gyro_z); 791 | this->get_parameter("imu.scale.accelerometer",acc_scale); 792 | this->get_parameter("imu.scale.gyro",gyro_scale); 793 | } 794 | } 795 | 796 | void setImuScales(const float acc, const float gyro){ 797 | std::vector serial_msg; 798 | uint8_t dim=packeter.packetC2F('G',acc,gyro); 799 | for(uint8_t i=0; iport()->async_send(serial_msg); 803 | serial_msg.clear(); 804 | } 805 | 806 | public: 807 | 808 | AMR_Node():Node("AMR_node"),node_ctx(new IoContext(2)),serial_driver(new drivers::serial_driver::SerialDriver(*node_ctx)),packeter(100){ 809 | vx=0.0; 810 | vy=0.0; 811 | w=0.0; 812 | x=0.0; 813 | y=0.0; 814 | theta=0.0; 815 | dt=0.0; 816 | dtheta=0.0; 817 | dx=0.0; 818 | dy=0.0; 819 | battery=0.0; 820 | battery_percentage=0.0; 821 | 822 | w_1=0.0; 823 | w_2=0.0; 824 | w_3=0.0; 825 | w_4=0.0; 826 | 827 | joint_data_available=false; 828 | to_be_publish=false; 829 | imu_data_available=false; 830 | battery_data_available=false; 831 | 832 | connected = false; 833 | 834 | cmd_is_exec=false; 835 | closing_is_exec=false; 836 | ask_to_close=false; 837 | 838 | // Parameters 839 | parameters_declaration(); 840 | get_all_parameters(); 841 | model_parameters(); 842 | imu_parameters(); 843 | parameters_callback_handle = add_on_set_parameters_callback(std::bind(&AMR_Node::parameters_callback, this, std::placeholders::_1)); 844 | 845 | 846 | 847 | try{ 848 | drivers::serial_driver::SerialPortConfig serial_config(baud_rate,drivers::serial_driver::FlowControl::NONE,drivers::serial_driver::Parity::NONE,drivers::serial_driver::StopBits::ONE); 849 | serial_driver->init_port(device_name, serial_config); 850 | 851 | previous_time=this->get_clock()->now(); 852 | 853 | if (!serial_driver->port()->is_open()){ 854 | serial_driver->port()->open(); 855 | } 856 | 857 | RCLCPP_INFO(get_logger(),"Serial opened on %s at %d",device_name.c_str(),baud_rate); 858 | 859 | 860 | //ROS2 stuffs 861 | timeout_time=this->get_clock()->now(); 862 | 863 | tf_bc = std::make_unique(*this); 864 | 865 | cmd_subscription = this->create_subscription("/amr/cmd_vel",10,std::bind(&AMR_Node::cmd_callback, this, std::placeholders::_1)); 866 | initial_pose_subscription = this->create_subscription("/amr/initial_pose",1,std::bind(&AMR_Node::initial_pose_callback, this, std::placeholders::_1)); 867 | 868 | odom_publisher = this->create_publisher("/amr/odometry",1); 869 | odom_timer = this->create_wall_timer(10ms, std::bind(&AMR_Node::odom_pub_callback, this)); 870 | 871 | if (publishImu){ 872 | imu_publisher = this->create_publisher("/amr/imu/raw",1); 873 | imu_timer = this->create_wall_timer(10ms, std::bind(&AMR_Node::imu_pub_callback, this)); 874 | } 875 | 876 | if (publishBattery){ 877 | battery_publisher = this->create_publisher("/amr/battery",1); 878 | battery_timer = this->create_wall_timer(1000ms, std::bind(&AMR_Node::battery_pub_callback, this)); 879 | } 880 | 881 | if (serial_driver->port()->is_open()){ 882 | serial_driver->port()->async_receive(std::bind(&AMR_Node::serial_callback, this, std::placeholders::_1, std::placeholders::_2)); 883 | connection_timer = this->create_wall_timer(1000ms, std::bind(&AMR_Node::check_connection, this)); 884 | } 885 | 886 | }catch(const std::exception & e){ 887 | RCLCPP_ERROR(get_logger(),"Error on creating serial port: %s",device_name.c_str()); 888 | this->~AMR_Node(); 889 | } 890 | } 891 | 892 | ~AMR_Node(){ 893 | closing_is_exec=false; 894 | ask_to_close=true; 895 | if (serial_driver->port()->is_open()){ 896 | std::vector serial_msg; 897 | uint8_t dim=packeter.packetC1F('S',0.0); 898 | for(uint8_t i=0; iport()->async_send(serial_msg); 904 | usleep(10000); 905 | } 906 | serial_msg.clear(); 907 | } 908 | 909 | 910 | if (node_ctx){ 911 | node_ctx->waitForExit(); 912 | } 913 | } 914 | 915 | 916 | }; 917 | 918 | 919 | 920 | #endif -------------------------------------------------------------------------------- /include/ucPack/CircularBuffer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * The MIT License 3 | * 4 | * Copyright (c) 2022 Giovanni di Dio Bruno https://gbr1.github.io 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to deal 8 | * in the Software without restriction, including without limitation the rights 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | * copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | * THE SOFTWARE. 23 | */ 24 | 25 | #include "CircularBuffer.h" 26 | 27 | 28 | CircularBuffer::CircularBuffer(const uint8_t dimension){ 29 | capacity=dimension; 30 | buffer = new uint8_t[dimension]; 31 | head=0; 32 | tail=0; 33 | size=0; 34 | } 35 | 36 | CircularBuffer::~CircularBuffer(){ 37 | delete buffer; 38 | } 39 | 40 | void CircularBuffer::push(const uint8_t element){ 41 | buffer[tail]=element; 42 | if (!isFull()){ 43 | size++; 44 | } 45 | tail=(tail+1)%capacity; 46 | if (size==capacity){ 47 | head=tail; 48 | } 49 | //std::cout<<"PUSH: "< 29 | 30 | class CircularBuffer { 31 | private: 32 | uint8_t * buffer; 33 | uint8_t head; 34 | uint8_t tail; 35 | uint8_t capacity; 36 | uint8_t size; 37 | public: 38 | CircularBuffer(const uint8_t dimension); 39 | ~CircularBuffer(); 40 | 41 | bool isEmpty(); 42 | bool isFull(); 43 | 44 | void push(const uint8_t element); 45 | uint8_t pop(); 46 | uint8_t top(); 47 | 48 | //void show(); 49 | 50 | void insert(const uint8_t * to_be_copied, const uint8_t dimension); 51 | 52 | uint8_t getSize(); 53 | 54 | //void copy(uint8_t * copied, uint8_t & dimension); 55 | 56 | uint8_t& operator[](int); 57 | 58 | uint8_t * ptr(); 59 | 60 | }; 61 | 62 | 63 | #endif /* CircularBuffer_hpp */ 64 | -------------------------------------------------------------------------------- /include/ucPack/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Giovanni Bruno 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /include/ucPack/README.md: -------------------------------------------------------------------------------- 1 | # ucPack 2 | 3 | ucPack - Unified C++ Packeter 4 | 5 | --- 6 | 7 | It is possible to use this library with Arduino or any other generic C++ framework. 8 | 9 | 10 | 11 | ## How to use 12 | 13 | ```c++ 14 | // declare ucPack class 15 | ucPack packeter(100,0x41,0x23); 16 | 17 | // use the internal buffer 18 | packeter.buffer.push(); 19 | 20 | // to check if a correct message is arrived 21 | packeter.checkPayload(); 22 | 23 | 24 | // compose a byte + 4 float packet 25 | uint8_t size=packeter.packetC4F('C',1.23,-3.14,90.0,45.0); 26 | // now you can use packeter.msg as uint8_t * 27 | 28 | 29 | // to unpack 30 | char c; 31 | float f1,f2,f3,f4; 32 | packeter.unpacketC4F(c, f1, f2, f3, f4); 33 | 34 | ``` 35 | 36 | 37 | 38 | ## How it works 39 | 40 | A packet is managed as: 41 | 42 | | index_byte | message length | message | stop_byte | crc8 | 43 | |:--------------------:|:--------------:|:---------------------------:|:--------------------:|:------:| 44 | | 1 byte (_default A_) | 1 byte | N bytes from message length | 1 byte (_default #_) | 1 byte | 45 | 46 | 47 | 48 | ## Todo 49 | 50 | - check all functions 51 | 52 | - add variadic compose and unpack 53 | 54 | 55 | 56 | > ***Copyright © 2022 Giovanni di Dio Bruno under MIT License.*** 57 | -------------------------------------------------------------------------------- /include/ucPack/ucPack.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * The MIT License 3 | * 4 | * Copyright (c) 2022 Giovanni di Dio Bruno https://gbr1.github.io 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to deal 8 | * in the Software without restriction, including without limitation the rights 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | * copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | * THE SOFTWARE. 23 | */ 24 | 25 | #include "ucPack.h" 26 | 27 | ucPack::ucPack(const uint8_t buffer_size, const uint8_t start_index, const uint8_t end_index):buffer(buffer_size){ 28 | this->start_index=start_index; 29 | this->end_index=end_index; 30 | payload=new uint8_t[buffer_size]; 31 | payload_size = 0; 32 | msg = new uint8_t[buffer_size]; 33 | msg_size = 0; 34 | } 35 | 36 | ucPack::~ucPack(){ 37 | delete [] msg; 38 | delete [] payload; 39 | } 40 | 41 | bool ucPack::checkPayload(){ 42 | //check if buffer is empty 43 | if (buffer.isEmpty()){ 44 | return false; 45 | } 46 | 47 | //check the index byte 48 | while ((buffer.top()!=start_index)&&(buffer.getSize()>0)){ 49 | buffer.pop(); //discard the first byte 50 | } 51 | 52 | //exit if only message index is received 53 | if (buffer.getSize()<=1){ 54 | return false; 55 | } 56 | 57 | //get the payload dimension 58 | payload_size=buffer[1]; 59 | 60 | //check if the packet is complete 61 | if (buffer.getSize()<(4+payload_size)){ //memo: index|length|msg|stop|crc8 62 | return false; 63 | } 64 | 65 | //check if stop byte is correct 66 | if (buffer[2+payload_size]!=end_index){ 67 | return false; 68 | } 69 | 70 | //crc checking 71 | //memcpy(payload,buffer.ptr()+2,payload_size); 72 | for(uint8_t i=0; i0; j--){ 105 | sum = (crc ^ extract) & 0x01; 106 | crc >>= 1; 107 | if (sum){ 108 | crc ^= 0x8C; 109 | } 110 | extract >>= 1; 111 | } 112 | data++; 113 | } 114 | return crc; 115 | } 116 | 117 | uint8_t ucPack::payloadTop(){ 118 | return payload[0]; 119 | } 120 | 121 | /* 122 | uint8_t ucPack::packetize(const char *types, const uint8_t n, ...){ 123 | msg[0]=start_index; 124 | va_list vl; 125 | uint8_t msg_size=2; 126 | for (uint8_t i=0; i 30 | #include 31 | //#include 32 | 33 | class ucPack { 34 | private: 35 | uint8_t start_index; 36 | uint8_t end_index; 37 | uint8_t * payload; 38 | uint8_t payload_size; 39 | uint8_t msg_size; 40 | public: 41 | uint8_t * msg; 42 | CircularBuffer buffer; 43 | ucPack(const uint8_t buffer_size, const uint8_t start_index='A', const uint8_t end_index='#'); 44 | 45 | bool checkPayload(); 46 | //void show(); 47 | 48 | uint8_t crc8(const uint8_t * data,const uint8_t size); 49 | uint8_t payloadTop(); 50 | //uint8_t packetize(const char * types, const uint8_t n, ...); //put bytes in msg and return msg_size; 51 | 52 | uint8_t packetC1F(const uint8_t code, const float f); 53 | void unpacketC1F(uint8_t &code, float &f); 54 | 55 | uint8_t packetC2F(const uint8_t code, const float f1, const float f2); 56 | void unpacketC2F(uint8_t &code, float &f1, float &f2); 57 | 58 | uint8_t packetC4F(const uint8_t code, const float f1, const float f2, const float f3, const float f4); 59 | void unpacketC4F(uint8_t &code, float &f1, float &f2, float &f3, float &f4); 60 | 61 | uint8_t packetC8F(const uint8_t code, const float f1, const float f2, const float f3, const float f4, 62 | const float f5, const float f6, const float f7, const float f8); 63 | void unpacketC8F(uint8_t &code, float &f1, float &f2, float &f3, float &f4, 64 | float &f5, float &f6, float &f7, float &f8); 65 | 66 | ~ucPack(); 67 | }; 68 | 69 | #endif /* ucPack_hpp */ 70 | -------------------------------------------------------------------------------- /launch/__pycache__/minimal_launch.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gbr1/ros2_amr_interface/64dfb5c725034b599d450d61b837e637468c412b/launch/__pycache__/minimal_launch.cpython-38.pyc -------------------------------------------------------------------------------- /launch/minimal_launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | def generate_launch_description(): 5 | return LaunchDescription([ 6 | Node( 7 | package='ros2_amr_interface', 8 | executable='amr_interface_node', 9 | name='amr', 10 | remappings=[ 11 | ('/amr/cmd_vel', '/cmd_vel') 12 | ], 13 | parameters=[{ 14 | 'try_reconnect': False 15 | }] 16 | ) 17 | ]) -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ros2_amr_interface 5 | 1.2.0 6 | Lightweight and flexible package dedicated to AMRs 7 | giovannididio.bruno@gmail.com 8 | MIT license 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | serial_driver 14 | geometry_msgs 15 | nav_msgs 16 | tf2 17 | tf2_ros 18 | sensor_msgs 19 | 20 | ros2launch 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /rviz/test_rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Odometry1 10 | - /Odometry1/Topic1 11 | - /Odometry1/Shape1 12 | - /TF1 13 | Splitter Ratio: 0.5 14 | Tree Height: 739 15 | - Class: rviz_common/Selection 16 | Name: Selection 17 | - Class: rviz_common/Tool Properties 18 | Expanded: 19 | - /2D Goal Pose1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz_common/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Alpha: 0.5 32 | Cell Size: 1 33 | Class: rviz_default_plugins/Grid 34 | Color: 160; 160; 164 35 | Enabled: true 36 | Line Style: 37 | Line Width: 0.029999999329447746 38 | Value: Lines 39 | Name: Grid 40 | Normal Cell Count: 0 41 | Offset: 42 | X: 0 43 | Y: 0 44 | Z: 0 45 | Plane: XY 46 | Plane Cell Count: 10 47 | Reference Frame: 48 | Value: true 49 | - Angle Tolerance: 0.10000000149011612 50 | Class: rviz_default_plugins/Odometry 51 | Covariance: 52 | Orientation: 53 | Alpha: 0.5 54 | Color: 255; 255; 127 55 | Color Style: Unique 56 | Frame: Local 57 | Offset: 1 58 | Scale: 1 59 | Value: true 60 | Position: 61 | Alpha: 0.30000001192092896 62 | Color: 204; 51; 204 63 | Scale: 1 64 | Value: true 65 | Value: true 66 | Enabled: true 67 | Keep: 1 68 | Name: Odometry 69 | Position Tolerance: 0.10000000149011612 70 | Shape: 71 | Alpha: 1 72 | Axes Length: 1 73 | Axes Radius: 0.10000000149011612 74 | Color: 252; 233; 79 75 | Head Length: 0.05000000074505806 76 | Head Radius: 0.03500000014901161 77 | Shaft Length: 0.30000001192092896 78 | Shaft Radius: 0.019999999552965164 79 | Value: Arrow 80 | Topic: 81 | Depth: 1 82 | Durability Policy: System Default 83 | History Policy: Keep Last 84 | Reliability Policy: System Default 85 | Value: /amr/odometry 86 | Value: true 87 | - Class: rviz_default_plugins/TF 88 | Enabled: true 89 | Frame Timeout: 15 90 | Frames: 91 | All Enabled: true 92 | base_link: 93 | Value: true 94 | odom: 95 | Value: true 96 | Marker Scale: 1 97 | Name: TF 98 | Show Arrows: true 99 | Show Axes: true 100 | Show Names: true 101 | Tree: 102 | odom: 103 | base_link: 104 | {} 105 | Update Interval: 0 106 | Value: true 107 | Enabled: true 108 | Global Options: 109 | Background Color: 48; 48; 48 110 | Fixed Frame: odom 111 | Frame Rate: 30 112 | Name: root 113 | Tools: 114 | - Class: rviz_default_plugins/Interact 115 | Hide Inactive Objects: true 116 | - Class: rviz_default_plugins/MoveCamera 117 | - Class: rviz_default_plugins/Select 118 | - Class: rviz_default_plugins/FocusCamera 119 | - Class: rviz_default_plugins/Measure 120 | Line color: 128; 128; 0 121 | - Class: rviz_default_plugins/SetInitialPose 122 | Topic: 123 | Depth: 5 124 | Durability Policy: Volatile 125 | History Policy: Keep Last 126 | Reliability Policy: Reliable 127 | Value: /initialpose 128 | - Class: rviz_default_plugins/SetGoal 129 | Topic: 130 | Depth: 5 131 | Durability Policy: Volatile 132 | History Policy: Keep Last 133 | Reliability Policy: Reliable 134 | Value: /goal_pose 135 | - Class: rviz_default_plugins/PublishPoint 136 | Single click: true 137 | Topic: 138 | Depth: 5 139 | Durability Policy: Volatile 140 | History Policy: Keep Last 141 | Reliability Policy: Reliable 142 | Value: /clicked_point 143 | Transformation: 144 | Current: 145 | Class: rviz_default_plugins/TF 146 | Value: true 147 | Views: 148 | Current: 149 | Class: rviz_default_plugins/Orbit 150 | Distance: 2.312662124633789 151 | Enable Stereo Rendering: 152 | Stereo Eye Separation: 0.05999999865889549 153 | Stereo Focal Distance: 1 154 | Swap Stereo Eyes: false 155 | Value: false 156 | Focal Point: 157 | X: 0 158 | Y: 0 159 | Z: 0 160 | Focal Shape Fixed Size: true 161 | Focal Shape Size: 0.05000000074505806 162 | Invert Z Axis: false 163 | Name: Current View 164 | Near Clip Distance: 0.009999999776482582 165 | Pitch: 0.8453978300094604 166 | Target Frame: 167 | Value: Orbit (rviz) 168 | Yaw: 2.4704043865203857 169 | Saved: ~ 170 | Window Geometry: 171 | Displays: 172 | collapsed: false 173 | Height: 968 174 | Hide Left Dock: false 175 | Hide Right Dock: false 176 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000036efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000036e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000395000000160000000000000000000000010000010f0000036efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000036e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003990000036e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 177 | Selection: 178 | collapsed: false 179 | Tool Properties: 180 | collapsed: false 181 | Views: 182 | collapsed: false 183 | Width: 1546 184 | X: 264 185 | Y: 195 186 | -------------------------------------------------------------------------------- /src/amr_interface_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * The MIT License 3 | * 4 | * Copyright (c) 2022 Giovanni di Dio Bruno https://gbr1.github.io 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to deal 8 | * in the Software without restriction, including without limitation the rights 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | * copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | * THE SOFTWARE. 23 | */ 24 | 25 | #include "rclcpp/rclcpp.hpp" 26 | #include "ros2_amr_interface/amr_node_class.hpp" 27 | 28 | void sigIntHandler(int sig){ 29 | rclcpp::shutdown(); 30 | (void)(sig); 31 | } 32 | 33 | 34 | int main(int argc, char ** argv){ 35 | rclcpp::init(argc, argv); 36 | auto node = std::make_shared(); 37 | signal(SIGINT, sigIntHandler); 38 | try{ 39 | rclcpp::spin(node); 40 | rclcpp::shutdown(); 41 | } 42 | catch(const std::exception & e){ 43 | RCLCPP_ERROR(node->get_logger(),"Error: %s",e.what()); 44 | } 45 | 46 | return 0; 47 | } 48 | --------------------------------------------------------------------------------