diff --git a/mg_control/CMakeLists.txt b/mg_control/CMakeLists.txt
new file mode 100644
index 0000000..3627758
--- /dev/null
+++ b/mg_control/CMakeLists.txt
@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.8)
+project(mg_control)
+
+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)
+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_control
+ SHARED
+ src/mg_control.cpp
+)
+
+target_include_directories(
+ mg_control
+ PRIVATE
+ ${LIBSERIAL_INCLUDE_DIRS}
+ include
+)
+
+target_link_libraries(
+ mg_control
+ ${LIBSERIAL_LIBRARIES}
+)
+
+ament_target_dependencies(
+ mg_control
+ rclcpp
+ pluginlib
+ hardware_interface
+ rclcpp_lifecycle
+)
+pluginlib_export_plugin_description_file(hardware_interface mg_control_interfaces.xml)
+
+install(
+ TARGETS mg_control
+ DESTINATION lib
+)
+
+install(DIRECTORY
+ launch assets
+ DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()
diff --git a/mg_control/assets/mg_base.urdf b/mg_control/assets/mg_base.urdf
new file mode 100644
index 0000000..362d87d
--- /dev/null
+++ b/mg_control/assets/mg_base.urdf
@@ -0,0 +1,71 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ mg_control/MgStepperInterface
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/mg_control/assets/mg_base_controllers.yaml b/mg_control/assets/mg_base_controllers.yaml
new file mode 100644
index 0000000..0883a17
--- /dev/null
+++ b/mg_control/assets/mg_base_controllers.yaml
@@ -0,0 +1,22 @@
+controller_manager:
+ ros__parameters:
+ update_rate: 100
+
+ diffdrive_controller:
+ type: diff_drive_controller/DiffDriveController
+ joint_state_publisher:
+ type: joint_state_broadcaster/JointStateBroadcaster
+
+diffdrive_controller:
+ ros__parameters:
+ left_wheel_names: ["left_motor"]
+ right_wheel_names: ["right_motor"]
+
+ enable_odom_tf: true
+ odom_frame_id: odom_excpected
+ base_frame_id: base-link
+
+ open_loop: true
+
+ wheel_separation: 0.258
+ wheel_radius: 0.0375
\ No newline at end of file
diff --git a/mg_control/assets/mg_base_controllers_local.yaml b/mg_control/assets/mg_base_controllers_local.yaml
new file mode 100644
index 0000000..cde0a7a
--- /dev/null
+++ b/mg_control/assets/mg_base_controllers_local.yaml
@@ -0,0 +1,22 @@
+controller_manager:
+ ros__parameters:
+ update_rate: 100
+
+ diffdrive_controller:
+ type: diff_drive_controller/DiffDriveController
+ joint_state_publisher:
+ type: joint_state_broadcaster/JointStateBroadcaster
+
+diffdrive_controller:
+ ros__parameters:
+ left_wheel_names: ["left_motor"]
+ right_wheel_names: ["right_motor"]
+
+ enable_odom_tf: true
+ odom_frame_id: odom
+ base_frame_id: base-link
+
+ open_loop: true
+
+ wheel_separation: 0.258
+ wheel_radius: 0.075
\ No newline at end of file
diff --git a/mg_control/assets/mg_base_local.urdf b/mg_control/assets/mg_base_local.urdf
new file mode 100644
index 0000000..95f38a5
--- /dev/null
+++ b/mg_control/assets/mg_base_local.urdf
@@ -0,0 +1,72 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ mock_components/GenericSystem
+ true
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/mg_control/include/mg_control/mg_control.hpp b/mg_control/include/mg_control/mg_control.hpp
new file mode 100644
index 0000000..aa0253d
--- /dev/null
+++ b/mg_control/include/mg_control/mg_control.hpp
@@ -0,0 +1,37 @@
+#ifndef MG_WHEEL_INTERFACE_HPP_
+#define MG_WHEEL_INTERFACE_HPP_
+#include
+#include
+
+
+#include "libserial/SerialPort.h"
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/system_interface.hpp"
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "pluginlib/class_list_macros.hpp"
+
+
+namespace mg {
+ class MgStepperInterface : public hardware_interface::SystemInterface {
+ public:
+ CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;
+ CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;
+ CallbackReturn on_shutdown(const rclcpp_lifecycle::State &) override;
+ std::vector export_state_interfaces() override;
+ std::vector export_command_interfaces() override;
+ hardware_interface::return_type read(const rclcpp::Time &time, const rclcpp::Duration &period) override;
+ hardware_interface::return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override;
+ private:
+ std::string serial_port_name;
+ LibSerial::SerialPort odrive_serial_interface;
+ double left_wheel_vel_cmd;
+ double left_wheel_pos_state;
+ double right_wheel_vel_cmd;
+ double right_wheel_pos_state;
+ };
+}
+
+PLUGINLIB_EXPORT_CLASS(mg::MgStepperInterface, hardware_interface::SystemInterface)
+
+#endif
\ No newline at end of file
diff --git a/mg_control/launch/launch.py b/mg_control/launch/launch.py
new file mode 100644
index 0000000..eebe7f0
--- /dev/null
+++ b/mg_control/launch/launch.py
@@ -0,0 +1,67 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction
+from launch.event_handlers import OnProcessStart
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from pathlib import Path
+
+
+
+def generate_launch_description():
+
+ is_local_test = DeclareLaunchArgument(
+ 'local_test',
+ default_value="False",
+ description='Launch with simulated components'
+ )
+
+ basedir = FindPackageShare("mg_control")
+ urdfDescription = PathJoinSubstitution([basedir,
+ "assets",
+ PythonExpression([
+ "'mg_base_local.urdf' if ",
+ LaunchConfiguration('local_test'),
+ " else 'mg_base.urdf'"])
+ ])
+ yamlControllers = PathJoinSubstitution([basedir,
+ "assets",
+ PythonExpression([
+ "'mg_base_controllers_local.yaml' if( ",
+ LaunchConfiguration('local_test'),
+ ") else 'mg_base_controllers.yaml'"])
+ ])
+
+ urdf = PythonExpression(["open('",urdfDescription, "').read()"])
+
+ return LaunchDescription([
+ is_local_test,
+ Node(
+ package='robot_state_publisher',
+ executable='robot_state_publisher',
+ output="log",
+ parameters=[{"robot_description": urdf}]
+ ),
+ Node(
+ package="controller_manager",
+ executable="ros2_control_node",
+ parameters=[yamlControllers],
+ output="log"
+ ),
+ Node(
+ package='controller_manager',
+ executable='spawner',
+ output="screen",
+ arguments=["joint_state_publisher"]
+ ),
+ Node(
+ package="controller_manager",
+ executable="spawner",
+ arguments=["diffdrive_controller",
+ "--param-file",
+ yamlControllers,
+ ]
+ )
+ ])
+
diff --git a/mg_control/mg_control_interfaces.xml b/mg_control/mg_control_interfaces.xml
new file mode 100644
index 0000000..8cb2a31
--- /dev/null
+++ b/mg_control/mg_control_interfaces.xml
@@ -0,0 +1,7 @@
+
+
+
\ No newline at end of file
diff --git a/mg_control/package.xml b/mg_control/package.xml
new file mode 100644
index 0000000..ef63dfd
--- /dev/null
+++ b/mg_control/package.xml
@@ -0,0 +1,35 @@
+
+
+
+ mg_control
+ 0.0.1
+ Ros2 control compatible drivers for magrob
+ Pimpest
+ Apache-2.0
+
+ ament_cmake
+
+ ros2launch
+
+ rclcpp
+ rclcpp_lifecycle
+ rclcpp_components
+ pluginlib
+
+ ros2_control
+ hardware_interface
+
+ controller_manager
+ diff_drive_controller
+ robot_state_publisher
+ joint_state_broadcaster
+
+ libserial-dev
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
diff --git a/mg_control/src/mg_control.cpp b/mg_control/src/mg_control.cpp
new file mode 100644
index 0000000..45e1861
--- /dev/null
+++ b/mg_control/src/mg_control.cpp
@@ -0,0 +1,99 @@
+#include "mg_control/mg_control.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+
+#include
+
+CallbackReturn mg::MgStepperInterface::on_init(const hardware_interface::HardwareInfo &info)
+{
+ if(hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS){
+ return CallbackReturn::ERROR;
+ }
+
+
+ if(info_.hardware_parameters.find("device_path") != info.hardware_parameters.end()) {
+ serial_port_name = info_.hardware_parameters["device_path"];
+ } else {
+ serial_port_name = "/dev/ttyACM0";
+ }
+
+ left_wheel_pos_state = 0;
+ right_wheel_pos_state = 0;
+ left_wheel_vel_cmd = 0;
+ right_wheel_vel_cmd = 0;
+ return CallbackReturn::SUCCESS;
+}
+
+
+CallbackReturn mg::MgStepperInterface::on_configure(const rclcpp_lifecycle::State &)
+{
+ try{
+ if(!odrive_serial_interface.IsOpen()){
+ odrive_serial_interface.Open(serial_port_name);
+ }
+ }
+ catch (const LibSerial::OpenFailed &er) {
+ RCLCPP_FATAL(rclcpp::get_logger("MgStepperInterface"),
+ "Failed to open serial port (Is the stepper driver plugged in)");
+ return CallbackReturn::ERROR;
+ }
+ return CallbackReturn::SUCCESS;
+}
+
+
+CallbackReturn mg::MgStepperInterface::on_shutdown(const rclcpp_lifecycle::State &)
+{
+ if(odrive_serial_interface.IsOpen()){
+ odrive_serial_interface.Close();
+ }
+ return CallbackReturn::SUCCESS;
+}
+
+
+std::vector mg::MgStepperInterface::export_state_interfaces()
+{
+ std::vector state_interfaces;
+ state_interfaces.reserve(2);
+ state_interfaces.push_back(hardware_interface::StateInterface(
+ "left_motor", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state
+ ));
+ state_interfaces.push_back(hardware_interface::StateInterface(
+ "right_motor", hardware_interface::HW_IF_POSITION, &right_wheel_pos_state
+ ));
+ return state_interfaces;
+}
+
+
+std::vector mg::MgStepperInterface::export_command_interfaces()
+{
+ std::vector command_interfaces;
+ command_interfaces.reserve(2);
+ command_interfaces.push_back(hardware_interface::CommandInterface(
+ "left_motor", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd
+ ));
+ command_interfaces.push_back(hardware_interface::CommandInterface(
+ "right_motor", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd
+ ));
+ return command_interfaces;
+}
+
+hardware_interface::return_type mg::MgStepperInterface::read(const rclcpp::Time &, const rclcpp::Duration &)
+{
+ return hardware_interface::return_type::OK ;
+}
+
+
+hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time &, const rclcpp::Duration &)
+{
+ std::string cmd_left;
+ std::string cmd_right;
+
+ cmd_left = std::to_string(left_wheel_vel_cmd/2*M_PI) + " ";
+ cmd_right = std::to_string(-right_wheel_vel_cmd/2*M_PI);
+
+ try {
+ odrive_serial_interface.Write(cmd_left + cmd_right);
+ } catch(const std::runtime_error &e){
+ return hardware_interface::return_type::ERROR;
+ }
+ return hardware_interface::return_type::OK;
+}