Merge mg_wheel_interface with magrob
This commit is contained in:
55
mg_control/CMakeLists.txt
Normal file
55
mg_control/CMakeLists.txt
Normal file
@ -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()
|
||||||
71
mg_control/assets/mg_base.urdf
Normal file
71
mg_control/assets/mg_base.urdf
Normal file
@ -0,0 +1,71 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot name="mg-robot">
|
||||||
|
|
||||||
|
|
||||||
|
<material name="red">
|
||||||
|
<color rgba="1 0 0 1" />
|
||||||
|
</material>
|
||||||
|
|
||||||
|
<material name="gray">
|
||||||
|
<color rgba="0.5 0.5 0.5 1" />
|
||||||
|
</material>
|
||||||
|
|
||||||
|
<link name="base-link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.24 0.32 0.12" color="yellow"/>
|
||||||
|
</geometry>
|
||||||
|
<origin xyz="0 0 0.12" rpy="0 0 0" />
|
||||||
|
<material name="red" />
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="left-wheel">
|
||||||
|
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder radius="0.075" length="0.05"/>
|
||||||
|
</geometry>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<material name="gray" />
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="right-wheel">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder radius="0.075" length="0.05"/>
|
||||||
|
</geometry>
|
||||||
|
<origin xyz="0 0 0." rpy="0 0 0" />
|
||||||
|
<material name="gray" />
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint name="right_motor" type="continuous">
|
||||||
|
<parent link="base-link"/>
|
||||||
|
<child link="right-wheel"/>
|
||||||
|
<origin xyz="0 -0.16 0.075" rpy="1.57 0 0" />
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="left_motor" type="continuous">
|
||||||
|
<parent link="base-link"/>
|
||||||
|
<child link="left-wheel"/>
|
||||||
|
<origin xyz="0 0.16 0.075" rpy="1.57 0 0" />
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
|
||||||
|
<ros2_control name="mg-base" type="system">
|
||||||
|
<hardware>
|
||||||
|
<plugin>mg_control/MgStepperInterface</plugin>
|
||||||
|
</hardware>
|
||||||
|
<joint name="left_motor">
|
||||||
|
<command_interface name="velocity"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="right_motor">
|
||||||
|
<command_interface name="velocity"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
</joint>
|
||||||
|
</ros2_control>
|
||||||
|
</robot>
|
||||||
22
mg_control/assets/mg_base_controllers.yaml
Normal file
22
mg_control/assets/mg_base_controllers.yaml
Normal file
@ -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
|
||||||
22
mg_control/assets/mg_base_controllers_local.yaml
Normal file
22
mg_control/assets/mg_base_controllers_local.yaml
Normal file
@ -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
|
||||||
72
mg_control/assets/mg_base_local.urdf
Normal file
72
mg_control/assets/mg_base_local.urdf
Normal file
@ -0,0 +1,72 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot name="mg-robot">
|
||||||
|
|
||||||
|
|
||||||
|
<material name="red">
|
||||||
|
<color rgba="1 0 0 1" />
|
||||||
|
</material>
|
||||||
|
|
||||||
|
<material name="gray">
|
||||||
|
<color rgba="0.5 0.5 0.5 1" />
|
||||||
|
</material>
|
||||||
|
|
||||||
|
<link name="base-link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size="0.24 0.32 0.12" color="yellow"/>
|
||||||
|
</geometry>
|
||||||
|
<origin xyz="0 0 0.12" rpy="0 0 0" />
|
||||||
|
<material name="red" />
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="left-wheel">
|
||||||
|
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder radius="0.075" length="0.05"/>
|
||||||
|
</geometry>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<material name="gray" />
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="right-wheel">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder radius="0.075" length="0.05"/>
|
||||||
|
</geometry>
|
||||||
|
<origin xyz="0 0 0." rpy="0 0 0" />
|
||||||
|
<material name="gray" />
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint name="right_motor" type="continuous">
|
||||||
|
<parent link="base-link"/>
|
||||||
|
<child link="right-wheel"/>
|
||||||
|
<origin xyz="0 -0.16 0.075" rpy="1.57 0 0" />
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="left_motor" type="continuous">
|
||||||
|
<parent link="base-link"/>
|
||||||
|
<child link="left-wheel"/>
|
||||||
|
<origin xyz="0 0.16 0.075" rpy="1.57 0 0" />
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
|
||||||
|
<ros2_control name="mg-base" type="system">
|
||||||
|
<hardware>
|
||||||
|
<plugin>mock_components/GenericSystem</plugin>
|
||||||
|
<param name="calculate_dynamics">true</param>
|
||||||
|
</hardware>
|
||||||
|
<joint name="left_motor">
|
||||||
|
<command_interface name="velocity"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="right_motor">
|
||||||
|
<command_interface name="velocity"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
</joint>
|
||||||
|
</ros2_control>
|
||||||
|
</robot>
|
||||||
37
mg_control/include/mg_control/mg_control.hpp
Normal file
37
mg_control/include/mg_control/mg_control.hpp
Normal file
@ -0,0 +1,37 @@
|
|||||||
|
#ifndef MG_WHEEL_INTERFACE_HPP_
|
||||||
|
#define MG_WHEEL_INTERFACE_HPP_
|
||||||
|
#include <vector>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
|
||||||
|
#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<hardware_interface::StateInterface> export_state_interfaces() override;
|
||||||
|
std::vector<hardware_interface::CommandInterface> 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
|
||||||
67
mg_control/launch/launch.py
Normal file
67
mg_control/launch/launch.py
Normal file
@ -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,
|
||||||
|
]
|
||||||
|
)
|
||||||
|
])
|
||||||
|
|
||||||
7
mg_control/mg_control_interfaces.xml
Normal file
7
mg_control/mg_control_interfaces.xml
Normal file
@ -0,0 +1,7 @@
|
|||||||
|
<library path="mg_control">
|
||||||
|
<class
|
||||||
|
name="mg_control/MgStepperInterface"
|
||||||
|
type="mg::MgStepperInterface"
|
||||||
|
base_class_type="hardware_interface::SystemInterface"
|
||||||
|
/>
|
||||||
|
</library>
|
||||||
35
mg_control/package.xml
Normal file
35
mg_control/package.xml
Normal file
@ -0,0 +1,35 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>mg_control</name>
|
||||||
|
<version>0.0.1</version>
|
||||||
|
<description>Ros2 control compatible drivers for magrob</description>
|
||||||
|
<maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
|
||||||
|
<license>Apache-2.0</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<depend>ros2launch</depend>
|
||||||
|
|
||||||
|
<depend>rclcpp</depend>
|
||||||
|
<depend>rclcpp_lifecycle</depend>
|
||||||
|
<depend>rclcpp_components</depend>
|
||||||
|
<depend>pluginlib </depend>
|
||||||
|
|
||||||
|
<depend>ros2_control</depend>
|
||||||
|
<depend>hardware_interface</depend>
|
||||||
|
|
||||||
|
<depend>controller_manager</depend>
|
||||||
|
<depend>diff_drive_controller</depend>
|
||||||
|
<depend>robot_state_publisher</depend>
|
||||||
|
<depend>joint_state_broadcaster</depend>
|
||||||
|
|
||||||
|
<depend>libserial-dev</depend>
|
||||||
|
|
||||||
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
99
mg_control/src/mg_control.cpp
Normal file
99
mg_control/src/mg_control.cpp
Normal file
@ -0,0 +1,99 @@
|
|||||||
|
#include "mg_control/mg_control.hpp"
|
||||||
|
#include "hardware_interface/types/hardware_interface_type_values.hpp"
|
||||||
|
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
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<hardware_interface::StateInterface> mg::MgStepperInterface::export_state_interfaces()
|
||||||
|
{
|
||||||
|
std::vector<hardware_interface::StateInterface> 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<hardware_interface::CommandInterface> mg::MgStepperInterface::export_command_interfaces()
|
||||||
|
{
|
||||||
|
std::vector<hardware_interface::CommandInterface> 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;
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user