Reverted the removed controller and fixed some bugs

This controller is based on @mateusmenezes95 3 wheel omnidirectional controller.

omnidrive_rppico/hardware/omnibot_pico_system.cpp

@@ -255,7 +255,7 @@ hardware_interface::return_type omnidrive_rppico ::OmniDriveRpPicoHardware::writ
   int motor_r_counts_per_loop = wheel_r_.cmd / wheel_r_.rads_per_count / cfg_.loop_rate;
 
 
-  
+
   comms_.set_motor_values(motor_f_counts_per_loop, 
                           motor_l_counts_per_loop,
                           motor_b_counts_per_loop, 

src/ezbot_robot/config/omnidirectional_controller.yaml

@@ -0,0 +1,35 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 30
+    #use_sim_time: true
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    omnidirectional_controller:
+      type: omnidirectional_controllers/OmnidirectionalController
+
+omnidirectional_controller:
+  ros__parameters:
+    wheel_names:
+      - roue_avant_joint
+      - roue_gauche_joint
+      - roue_arriere_joint
+      - roue_droite_joint
+
+    robot_radius: 0.1
+    wheel_radius: 0.053112205
+    gamma: 30.0 # angle between body fixed y axis and the normal of wheel3
+
+    publish_rate: 50.0
+    odom_frame_id: odom
+    base_frame_id: base_link
+    pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+    odom_numeric_integration_method: runge_kutta2
+
+    open_loop: true
+    enable_odom_tf: true
+
+    cmd_vel_timeout: 0.5
+    use_stamped_vel: false

src/ezbot_robot/config/ros2_controllers.yaml

@@ -0,0 +1,19 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100
+    use_sim_time: true
+
+    omnidirectional_controller:
+      type: forward_command_controller/ForwardCommandController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+omnidirectional_controller:
+  ros__parameters:
+    joints:
+      - roue_avant_joint
+      - roue_gauche_joint
+      - roue_arriere_joint
+      - roue_droite_joint
+    interface_name: velocity

src/ezbot_robot/description/robot_core.xacro

@@ -257,7 +257,7 @@
             <surface>
                 <contact>
                 <ode>
-                    <max_vel>0</max_vel>clear
+                    <max_vel>0</max_vel>
                     <min_depth>0.02</min_depth>
                 </ode>
                 </contact>

src/ezbot_robot/description/ros2_control.xacro

@@ -12,7 +12,7 @@
             <param name="right_wheel_name">roue_droite_joint</param>
 
             <param name="loop_rate">30</param>
-            <param name="device">/dev/ttyAMA1</param>
+            <param name="device">/dev/pts/11</param>
             <param name="baudrate">115200</param>
 
             <param name="timeout_ms">10000</param>

src/ezbot_robot/launch/real_robot.launch.py

@@ -45,13 +45,10 @@ def generate_launch_description():
     )
 
     #robot_description = Command(['ros2 param get --hide-type /robot_state_publisher robot_description'])
-    controller_params_file = os.path.join(get_package_share_directory('ezbot_robot'), 'config', 'controllers.yaml')
-
-    pkg_path = os.path.join(get_package_share_directory('ezbot_robot'))
-    xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
-    robot_description = {"robot_description": xacro.process_file(xacro_file).toxml()}
-
+    #controller_params_file = os.path.join(get_package_share_directory('ezbot_robot'), 'config', 'controllers.yaml')
+    controller_params_file = os.path.join(get_package_share_directory('ezbot_robot'), 'config', 'omnidirectional_controller.yaml')
 
+
     controller_manager = Node(
         package="controller_manager",
         executable="ros2_control_node",
@@ -75,7 +72,7 @@ def generate_launch_description():
         arguments=['omnidirectional_controller'],
     )
     delayed_omnidrive_spawner = TimerAction(
-        period=1.0,
+        period=10.0,
         actions=[omnidrive_spawner],
     )
 

src/omnidirectional_controllers/.gitattributes

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

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.