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