Zenn
📝

ros2_controlのコントローラを作成する手順

2025/02/26に公開
tech

ros2_controlのコントローラパッケージを作成する手順をまとめます。

パッケージの作成

my_robot_wsというワークスペース内にmy_robot_controllerパッケージを作成します。

cd ~/my_robot_ws/src
ros2 pkg create --build-type ament_cmake my_robot_controller

ヘッダーファイルの追加

include/my_robot_controllerディレクトリにヘッダーファイルを追加します。ファイル名はmy_robot_controller.hppとします。

#ifndef MY_ROBOT_CONTROLLER_HPP_
#define MY_ROBOT_CONTROLLER_HPP_

#include "controller_interface/controller_interface.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"

namespace my_robot_controller
{

class MyRobotController : public controller_interface::ControllerInterface
{
public:
  MyRobotController();

  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
  
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override;
  
  controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override;
};

} // namespace my_robot_controller

#endif // MY_ROBOT_CONTROLLER_HPP_

ソースコードの追加

srcディレクトリにソースコードを追加します。ファイル名はmy_robot_controller.cppとします。

#include "my_robot_controller/my_robot_controller.hpp"

namespace my_robot_controller
{
MyRobotController::MyRobotController()
{
}

controller_interface::InterfaceConfiguration MyRobotController::command_interface_configuration() const
{
  controller_interface::InterfaceConfiguration config;
  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
  return config;
}

controller_interface::InterfaceConfiguration MyRobotController::state_interface_configuration() const
{
  controller_interface::InterfaceConfiguration config;
  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
  return config;
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn MyRobotController::on_init()
{
  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

controller_interface::return_type MyRobotController::update(const rclcpp::Time &time, const rclcpp::Duration &period)
{
  return controller_interface::return_type::OK;
}
} // namespace my_robot_controller

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(my_robot_controller::MyRobotController, controller_interface::ControllerInterface)

my_robot_controller_plugin.xmlの作成

my_robot_controller_plugin.xmlというファイルを作成します。

<library path="my_robot_controller">
  <class name="my_robot_controller/MyRobotController" type="my_robot_controller::MyRobotController"
          base_class_type="controller_interface::ControllerInterface"
  >
    <description>
      My Robot Controller
    </description>
  </class>
</library>

package.xmlの編集

package.xmlに依存関係を追加します。

<depend>controller_interface</depend>
<depend>pluginlib</depend>

CMakeLists.txtの編集

CMakeLists.txtにコントローラパッケージを作成するための設定を追加します。

cmake_minimum_required(VERSION 3.8)
project(my_robot_controller)

find_package(ament_cmake REQUIRED)
find_package(controller_interface REQUIRED)
find_package(pluginlib REQUIRED)

add_library(my_robot_controller src/my_robot_controller.cpp)
target_include_directories(my_robot_controller PRIVATE include)
ament_target_dependencies(my_robot_controller controller_interface pluginlib)

pluginlib_export_plugin_description_file(controller_interface my_robot_controller_plugin.xml)

install(TARGETS
    my_robot_controller
    ARCHIVE DESTINATION lib
    LIBRARY DESTINATION lib
    RUNTIME DESTINATION bin
)

install(FILES
    my_robot_controller_plugin.xml
    DESTINATION share/${PROJECT_NAME}
)

install(DIRECTORY
    include/
    DESTINATION include
)

ament_export_include_directories(include)
ament_export_libraries(my_robot_controller)

ament_package()

bringupパッケージのlaunchファイルにコントローラを追加

my_robot_bringupパッケージのlaunchディレクトリにコントローラを追加するlaunchファイルを作成します。

touch ~/my_robot_ws/src/my_robot_bringup/launch/my_robot_controller.launch.py

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.substitutions import Command
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue

def generate_launch_description():

    description_file = get_package_share_directory('my_robot_description') + '/urdf/my_robot.urdf.xacro'

    robot_description_content = Command(['xacro ', description_file])

    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[{
            'use_sim_time': False,
            'robot_description': ParameterValue(robot_description_content, value_type=str),
        }]
    )

    joint_state_publisher = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        output='screen'
    )

    rviz2 = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen'
    )

    my_robot_controller = Node(
        package='controller_manager',
        executable='ros2_control_node',
        name='my_robot_controller',
        output='both'
    )

    joint_state_broadcaster = Node(
        package='controller_manager',
        executable='spawner',
        name='joint_state_broadcaster',
        arguments=["joint_state_broadcaster", "-c", "/controller_manager"]
    )

    return LaunchDescription([
        robot_state_publisher,
        joint_state_publisher,
        rviz2,
        my_robot_controller,
        joint_state_broadcaster
    ])

bringupパッケージにros2_controlのcontrollers.yamlを追加

my_robot_bringupパッケージのconfigディレクトリにcontrollers.yamlを追加します。

controller_manager:
  ros__parameters:
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
    my_robot_controller:
      type: my_robot_controller/MyRobotController
      joints:
        - joint1
        - joint2
        - joint3
        - joint4
        - joint5
        - joint6
        - joint7

bringupパッケージのCMakeLists.txtを編集

CMakeLists.txtにコントローラパッケージをビルドするための設定を追加します。

find_package(my_robot_controller REQUIRED)

install(DIRECTORY
    config
    DESTINATION share/${PROJECT_NAME}
)

my_robot_descriptionパッケージのCMakeLists.txtを編集

my_robot_descriptionパッケージのCMakeLists.txtにコントローラパッケージをビルドするための設定を追加します。

find_package(my_robot_controller REQUIRED)
ament_target_dependencies(my_robot_description my_robot_controller)

my_robot_descriptionのurdfファイルにコントローラを追加

my_robot_descriptionパッケージのurdfファイルにros2_controlの記述を追加します。

    <!-- ros2_control Joint Macro -->
    <xacro:macro name="control_joint" params="name">
        <joint name="${name}">
            <command_interface name="position" />
            <state_interface name="position" />
            <state_interface name="velocity" />
        </joint>
    </xacro:macro>

    <!-- ros2_control Joints -->
    <ros2_control name="MyRobotHardware" type="system">
        <hardware>
            <plugin>mock_components/GenericSystem</plugin>
        </hardware>
        <!-- ジョイントのインターフェースを定義 -->
        <xacro:control_joint name="joint1" />
        <xacro:control_joint name="joint2" />
        <xacro:control_joint name="joint3" />
        <xacro:control_joint name="joint4" />
        <xacro:control_joint name="joint5" />
        <xacro:control_joint name="joint6" />
        <xacro:control_joint name="joint7" />
    </ros2_control>

my_robot_bringupパッケージのlaunchファイルを編集

my_robot_bringupパッケージのlaunchディレクトリにあるmy_robot_controller.launch.pyを編集します。

    ...
    my_robot_controller = Node(
        package='controller_manager',
        executable='ros2_control_node',
        name='controller_manager',
        output='both',
        parameters=[controllers_file,
                    {
                        'use_sim_time': False,
                        'robot_description': ParameterValue(robot_description_content, value_type=str),
                    }]
    )
    ...

ビルドと実行

ビルドします。

cd ~/my_robot_ws
colcon build

コントローラを起動します。

ros2 launch my_robot_bringup my_robot_controller.launch.py

Discussion