Added omnidirectional controller from axebot

src/omnidirectional_controllers/.gitattributes

@@ -0,0 +1 @@
+*.png filter=lfs diff=lfs merge=lfs -text

src/omnidirectional_controllers/.github/CODEOWNERS

@@ -0,0 +1,5 @@
+# These owners will be the default owners for everything in
+# the repo. Unless a later match takes precedence,
+# @global-owner1 and @global-owner2 will be requested for
+# review when someone opens a pull request.
+* @mateusmenezes95

src/omnidirectional_controllers/.github/workflows/humble-source-build.yaml

@@ -0,0 +1,20 @@
+name: Humble Source Build
+
+on:
+  pull_request:
+    branches:
+      - humble
+
+jobs:
+  humble_source_build:
+    runs-on: ubuntu-22.04
+    steps:
+      - uses: ros-tooling/[email protected]
+        with:
+          rosdistro: humble
+      - uses: ros-tooling/[email protected]
+        with:
+          package-name: omnidirectional_controllers
+          ref: ${{ github.event.pull_request.head.sha }}
+          target-ros2-distro: humble
+          skip-tests: true

src/omnidirectional_controllers/.gitignore

@@ -0,0 +1,55 @@
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE
+
+
+# vscode folder
+.vscode

src/omnidirectional_controllers/CMakeLists.txt

@@ -0,0 +1,139 @@
+cmake_minimum_required(VERSION 3.5)
+project(omnidirectional_controllers)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  controller_interface
+  geometry_msgs
+  hardware_interface
+  nav_msgs
+  pluginlib
+  rclcpp_lifecycle
+  rclcpp
+  tf2_msgs
+  tf2_ros
+  tf2
+)
+
+find_package(ament_cmake REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+add_library(odometry SHARED)
+
+target_sources(odometry 
+  PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/odometry.cpp
+)
+
+target_include_directories(odometry
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+add_library(kinematics SHARED)
+
+target_sources(kinematics 
+  PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/kinematics.cpp
+)
+
+target_include_directories(kinematics
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+add_library(${PROJECT_NAME} SHARED
+  src/omnidirectional_controller.cpp
+)
+
+target_include_directories(${PROJECT_NAME}
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+
+target_link_libraries(${PROJECT_NAME}
+  odometry
+  kinematics
+)
+
+target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+pluginlib_export_plugin_description_file(controller_interface omnidirectional_plugin.xml)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+install(TARGETS ${PROJECT_NAME}
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_gmock REQUIRED)
+
+  ament_lint_auto_find_test_dependencies()
+
+  ament_add_gmock(
+    test_odometry
+    test/test_odometry.cpp
+    src/odometry.cpp
+  )
+
+  target_link_libraries(test_odometry
+    odometry
+    kinematics
+  )
+
+  ament_add_gmock(
+    test_kinematics
+    test/test_kinematics.cpp
+    src/kinematics.cpp
+  )
+
+  target_link_libraries(test_kinematics
+    odometry
+    kinematics
+  )
+
+  target_include_directories(test_odometry PRIVATE include)
+  ament_target_dependencies(test_odometry)
+
+  target_include_directories(test_kinematics PRIVATE include)
+  ament_target_dependencies(test_kinematics)
+endif()
+
+ament_export_dependencies(
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+
+ament_export_include_directories(
+  include
+)
+
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+
+ament_package()

src/omnidirectional_controllers/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2022 Mateus Menezes
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

src/omnidirectional_controllers/README.md

@@ -0,0 +1,104 @@
+# Omnidirectional Controllers
+
+This package provides ROS 2 controllers for controlling Omnidirectional robots with three wheels. It is based on the concepts of [ros2_control] and [ros2_controllers]. Initially, only forward and inverse kinematics, based solely on the [diff_drive_controller], and odometry calculation have been implemented. The input for control is robot body velocity ($\dot{x}$, $\dot{y}$, $\dot{\theta}$) commands, which are translated into wheel commands ($\omega_1$, $\omega_2$, $\omega_3$) for an omnidirectional robot with three wheels. Odometry is computed from hardware feedback and published. It is worth noting that there are plans to further develop advanced linear and non-linear controllers, such as Model Predictive Control (MPC).
+
+**Author:** Mateus Menezes<br />
+**Maintainer:** Mateus Menezes, [email protected]
+
+## Build status
+
+ROS2 Distro | Branch | Build status |
+:---------: | :----: | :----------: |
+**Humble** | [`humble`](https://github.com/mateusmenezes95/omnidirectional_controllers/tree/humble) | [![Build From Source](https://github.com/mateusmenezes95/omnidirectional_controllers/actions/workflows/humble-source-build.yaml/badge.svg)](https://github.com/mateusmenezes95/omnidirectional_controllers/actions/workflows/humble-source-build.yaml)
+
+## Installation Premises
+
+1. This repository has been tested on [ROS2 Foxy] and with [Classic Gazebo 11];
+
+2. These instructions assume that you have already installed ROS2 Foxy Fitzroy on your machine. If not, please follow the recommended [recommended ubuntu installation tutorial];
+
+3. Before installing the package, you will need to have an ament workspace set up. If you don't have one, follow the instructions in the [Creating a workspace tutorial]. Once you have created the workspace, clone this repository in the source folder of your workspace.
+
+## Installation
+
+> **ATTENTION:** These commands assume that you have created a workspace called "ros_ws" in your home folder. If you used a different directory or name, please adjust the commands accordingly.
+
+After installing ROS2 and creating the workspace, clone this repository in your workspace:
+
+```
+cd ~/ros_ws/src
+git clone https://github.com/mateusmenezes95/omnidirectional_controllers
+```
+
+Install the binary dependencies by running the following command in the root of your workspace:
+
+```
+cd ~/ros_ws
+rosdep init
+rosdep update
+sudo apt update
+rosdep install --from-paths src/omnidirectional_controllers --ignore-src -r -y --rosdistro foxy
+```
+
+If all dependencies are already installed, you should see the message "All required rosdeps installed successfully."
+
+## Building
+
+Run the following command to build the package:
+
+```
+cd ~/ros_ws
+colcon build --symlink-install --event-handlers console_direct+
+```
+
+> Run `colcon build --help` to understand the arguments passed!
+
+After building the package, open a new terminal and navigate to your workspace. Then, source the overlay by running the following command:
+
+```
+source /opt/ros/foxy/setup.bash
+cd ~/ros_ws
+. install/local_setup.bash
+```
+
+> See [Source the overlay] to learn about underlay and overlay concepts.
+
+## Usage
+
+You must follow the three steps explained in [Running the Framework for Your Robot](https://control.ros.org/master/doc/getting_started/getting_started.html#running-the-framework-for-your-robot) tutorial.
+
+For an concrete example of how to use the Omnidirectional controllers, refer to the Axebot simulation's [controller configuration], [ros2_control URDF], and [launch file].
+
+### Subscribed Topic
+
+* **`/omnidirectional_controller/cmd_vel_unstamped`** ([geometry_msgs/msg/Twist])
+
+    Velocity twist from which the controller extracts the x and y component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored.
+
+### Published Topic
+
+* **`/omnidirectional_controller/odom`** ([nav_msgs/msg/Odometry])
+
+    Robot odometry. The odometry can be computed from hardware feedback or using an open-loop approach, in which the integration is performed using the Twist command. You can select the approach in the configuration file. Additionally, you can choose either the Runge-Kutta or Euler Forward integration method.
+
+## Unit Test
+
+> TODO
+
+## Bugs & Feature Requests
+
+Please report bugs and request features using the [Issue Tracker]
+
+[ros2_control]: https://control.ros.org/master/index.html
+[ros2_controllers]: https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html
+[Issue Tracker]: https://github.com/mateusmenezes95/omnidirectional_controllers/issues
+[diff_drive_controller]: https://control.ros.org/master/doc/ros2_controllers/diff_drive_controller/doc/userdoc.html
+[ros2_control URDF]: https://github.com/mateusmenezes95/axebot/blob/foxy/axebot_description/urdf/ros2_control.urdf.xacro
+[controller configuration]: https://github.com/mateusmenezes95/axebot/blob/foxy/axebot_control/config/omnidirectional_controller.yaml
+[launch file]: https://github.com/mateusmenezes95/axebot/blob/foxy/axebot_gazebo/launch/axebot.launch.py
+[Creating a workspace tutorial]: https://docs.ros.org/en/foxy/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html#creating-a-workspace
+[recommended ubuntu installation tutorial]: https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html
+[Source the overlay]: https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html#source-the-overlay
+[geometry_msgs/msg/Twist]: https://docs.ros2.org/latest/api/geometry_msgs/msg/Twist.html
+[nav_msgs/msg/Odometry]: https://docs.ros2.org/latest/api/nav_msgs/msg/Odometry.html
+[Classic Gazebo 11]: https://classic.gazebosim.org/

src/omnidirectional_controllers/doc/images/.gitattributes

@@ -0,0 +1 @@
+*.jpg filter=lfs diff=lfs merge=lfs -text