From 7019f3db168dc178abfd9672c0237c94b196a072 Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+Pimpest@users.noreply.github.com>
Date: Fri, 18 Oct 2024 00:18:52 +0200
Subject: [PATCH] Made basic interfacing with odrive

---
 mg_wheel_interface/CMakeLists.txt             | 42 +++++++++++++++++--
 mg_wheel_interface/assets/mg_base.urdf        |  3 +-
 .../assets/mg_base_controllers.yaml           |  2 +
 mg_wheel_interface/package.xml                |  6 +++
 4 files changed, 48 insertions(+), 5 deletions(-)

diff --git a/mg_wheel_interface/CMakeLists.txt b/mg_wheel_interface/CMakeLists.txt
index 71437ab..7af0e95 100644
--- a/mg_wheel_interface/CMakeLists.txt
+++ b/mg_wheel_interface/CMakeLists.txt
@@ -7,9 +7,45 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_components REQUIRED)
+include(FindPkgConfig)
+pkg_search_module(LIBSERIAL REQUIRED libserial)
+
+add_library(
+  mg_wheel_interface
+  SHARED
+  src/mg_wheel_interface.cpp
+)
+
+target_include_directories(
+  mg_wheel_interface
+  PRIVATE
+  ${LIBSERIAL_INCLUDE_DIRS}
+  include
+)
+
+target_link_libraries(
+  mg_wheel_interface
+  ${LIBSERIAL_LIBRARIES}
+)
+
+ament_target_dependencies(
+  mg_wheel_interface
+  rclcpp
+  pluginlib
+  hardware_interface
+  rclcpp_lifecycle
+)
+pluginlib_export_plugin_description_file(hardware_interface mg_wheel_interface.xml)
+
+install(
+  TARGETS mg_wheel_interface
+  DESTINATION lib
+)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
diff --git a/mg_wheel_interface/assets/mg_base.urdf b/mg_wheel_interface/assets/mg_base.urdf
index f5259f4..377965c 100644
--- a/mg_wheel_interface/assets/mg_base.urdf
+++ b/mg_wheel_interface/assets/mg_base.urdf
@@ -57,8 +57,7 @@
 
     <ros2_control name="mg-base" type="system">
         <hardware>
-            <plugin>mock_components/GenericSystem</plugin>
-            <param name="calculate_dynamics">true</param>
+            <plugin>mg_wheel_interface/MgOdriveInterface</plugin>
         </hardware>
         <joint name="left_motor">
             <command_interface name="velocity"/>
diff --git a/mg_wheel_interface/assets/mg_base_controllers.yaml b/mg_wheel_interface/assets/mg_base_controllers.yaml
index 67fe895..3d269ed 100644
--- a/mg_wheel_interface/assets/mg_base_controllers.yaml
+++ b/mg_wheel_interface/assets/mg_base_controllers.yaml
@@ -16,5 +16,7 @@ diffdrive_controller:
     odom_frame_id: odom
     base_frame_id: base-link
 
+    open_loop: true 
+
     wheel_separation: 0.5
     wheel_radius: 0.1
\ No newline at end of file
diff --git a/mg_wheel_interface/package.xml b/mg_wheel_interface/package.xml
index 6ea63c5..993f230 100644
--- a/mg_wheel_interface/package.xml
+++ b/mg_wheel_interface/package.xml
@@ -9,6 +9,9 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>rclcpp_components</depend>
   <depend>pluginlib </depend>
   <depend>hardware_interface</depend>
   <depend>ros2launch</depend>
@@ -19,6 +22,9 @@
   <depend>joint_state_broadcaster</depend>
   <depend>rsl</depend>
 
+  <build_depend>libserial-dev</build_depend>
+  <exec_depend>libserial</exec_depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>