From b5d474ff015611c4ee16628dc067b764ddd9a8ad Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Sat, 22 Feb 2025 15:48:08 +0100 Subject: [PATCH] Merge mg_wheel_interface with magrob --- mg_control/CMakeLists.txt | 55 +++++++++++ mg_control/assets/mg_base.urdf | 71 +++++++++++++ mg_control/assets/mg_base_controllers.yaml | 22 +++++ .../assets/mg_base_controllers_local.yaml | 22 +++++ mg_control/assets/mg_base_local.urdf | 72 ++++++++++++++ mg_control/include/mg_control/mg_control.hpp | 37 +++++++ mg_control/launch/launch.py | 67 +++++++++++++ mg_control/mg_control_interfaces.xml | 7 ++ mg_control/package.xml | 35 +++++++ mg_control/src/mg_control.cpp | 99 +++++++++++++++++++ 10 files changed, 487 insertions(+) create mode 100644 mg_control/CMakeLists.txt create mode 100644 mg_control/assets/mg_base.urdf create mode 100644 mg_control/assets/mg_base_controllers.yaml create mode 100644 mg_control/assets/mg_base_controllers_local.yaml create mode 100644 mg_control/assets/mg_base_local.urdf create mode 100644 mg_control/include/mg_control/mg_control.hpp create mode 100644 mg_control/launch/launch.py create mode 100644 mg_control/mg_control_interfaces.xml create mode 100644 mg_control/package.xml create mode 100644 mg_control/src/mg_control.cpp 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; +}