🐡
ROS2: Gazebo上のロボットをC++で制御する
海洋ロボコンをやってた人です。
今回は、Gazebo上で表示したURDF記述のロボットをC++で制御する方法を紹介します。
Gazeboはじめましての方のお力添えになれば幸いです。
※2023/02/26追記: 本記事の「ROS2」表記、正しくは「ROS 2」です。
Gazebo上のロボットをC++で制御する
今回は、Gazebo上のロボットを制御する方法を紹介するので、まだURDFでロボットを表示できていない方は、こちらからご覧になることをオススメします。
また使用するモデルは、アイコンでも表示されているマンタ型ロボットで紹介していきます。
なお、ロボットのハード設計からソフト設計まで全て私が担当しており、所属研究室の教授からも承諾を得ています。
本記事のプログラム
下記のGithubにて公開しているので、こちらを参考にしてください。
スター頂けると、大変嬉しいです・ω・
Gazebo上で動かすための設定ファイル等を準備
Gazebo上で動かすためのyaml, urdf, launch.pyを準備していきます。
前回、xxxxx_gazebo_ros2_controlというパッケージを作成しているので、このパッケージへ必要なプログラムを追加する形ですね。
xxxxx_gazebo_ros2_controlのパッケージの作り方は以下参考
まずはgazebo_ros2_controlで読み込む設定ファイルを作成します。
前回と比較し、Moveitのコントローラーを使用する必要はないので、ジョイント軌道を指定し動作させるjoint_trajectory_controllerを使用して動かそうと思います。
そのため、config下に以下を作成します。
manta_v2_trajectory_controller.yaml
controller_manager:
ros__parameters:
update_rate: 100 # Hz
joint_trajectory_controller:
type: joint_trajectory_controller/JointTrajectoryController
joint_state_controller:
type: joint_state_controller/JointStateController
joint_trajectory_controller:
ros__parameters:
joints:
# Rev Link R
- rev_link_Rtop_roll1
- rev_link_Rtop_pitch
- rev_link_Rtop_roll2
- rev_link_Rmid_roll1
- rev_link_Rmid_pitch
- rev_link_Rmid_roll2
- rev_link_Rbtm_roll1
- rev_link_Rbtm_pitch
- rev_link_Rbtm_roll2
# Rev Link L
- rev_link_Ltop_roll1
- rev_link_Ltop_pitch
- rev_link_Ltop_roll2
- rev_link_Lmid_roll1
- rev_link_Lmid_pitch
- rev_link_Lmid_roll2
- rev_link_Lbtm_roll1
- rev_link_Lbtm_pitch
- rev_link_Lbtm_roll2
# Rev Link Head
- rev_link_head
command_interfaces:
- position
state_interfaces:
- position
- velocity
続いて、全てのジョイントをもつjoint_trajectory_controllerをgazebo_ros2_controlのパラメータへ設定します。
urdfフォルダ下に新たにxxxxx.urdfファイルを作成し、gazebo_ros2_controlプラグインのパラメータファイルを上記のyamlファイルへ変更します。
その他urdfの記述はmoveitとGazeboを連携したときの手法と同じです。（略）
manta_v2.urdf
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<robot_param>robot_description</robot_param>
<robot_param_node>robot_state_publisher</robot_param_node>
<!-- <parameters>$(find manta_v2_gazebo_ros2_control)/config/manta_v2_moveit_controller.yaml</parameters> -->
<parameters>$(find manta_v2_gazebo_ros2_control)/config/manta_v2_trajectory_controller.yaml</parameters>
</plugin>
</gazebo>
ロードするコントローラーの内容が変わったので、ローンチファイルも新たに作成します。
trajectory_controller.launch.py
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import xacro
import os
def generate_launch_description():
# Constants for paths to different files and folders
gazebo_pkg_name = 'manta_v2_gazebo_ros2_control'
robot_name_in_model = 'manta_v2'
urdf_file_path = 'urdf/manta_v2.urdf'
world_file_path = 'worlds/basic.world'
# Pose where we want to spawn the robot
spawn_x_val = '0.0'
spawn_y_val = '0.0'
spawn_z_val = '0.0'
spawn_yaw_val = '0.0'
gazebo_pkg_share = FindPackageShare(package=gazebo_pkg_name).find(gazebo_pkg_name)
world_path = os.path.join(gazebo_pkg_share, world_file_path)
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
launch_arguments={'world': world_path}.items(),
)
urdf_model_path = os.path.join(gazebo_pkg_share, urdf_file_path)
doc = xacro.parse(open(urdf_model_path))
xacro.process_doc(doc)
robot_description = {'robot_description': doc.toxml()}
gazebo_robot_state_pub_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[robot_description]
)
spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
arguments=['-topic', 'robot_description',
'-entity', robot_name_in_model,
'-x', spawn_x_val,
'-y', spawn_y_val,
'-z', spawn_z_val,
'-Y', spawn_yaw_val,
],
output='screen')
load_joint_state_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', 'joint_state_broadcaster'],
output='screen'
)
load_joint_trajectory_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_start_controller', 'joint_trajectory_controller'],
output='screen'
)
nodes = [
# Gazebo
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=spawn_entity,
on_exit=[load_joint_state_controller],
)
),
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=load_joint_state_controller,
on_exit=[
load_joint_trajectory_controller,
],
)
),
gazebo,
gazebo_robot_state_pub_node,
spawn_entity,
]
return LaunchDescription(nodes)
GazeboとMoveitを連携したときのプログラムと差異はほとんどありませんね。
urdfロボットを動かすためのC++プログラムを作成する
ここからがメインの内容になりますね。
まずは、manta_v2_gazebo_ros2_control/src下に
- example_position.cpp
というファイルを作成します。
基本的に、ros-control開発チームのGitHubのデモンストレーションを参考に、プログラムを作成しています。
上記のリポジトリをクローン後
- gazebo_ros2_control/gazebo_ros2_control_demos/examples/example_position.cpp
を手元のパッケージへコピーしておきましょう。
また、C++で制御する時のイメージですが
- launchでjoint_trajectory_controllerをロードする
- C++実行により、ジョイン軌道をAction Clientに送信する
- Action Clientがアクションの目標をAction Serverへ送信する
- コントローラーがロボットにコマンドを送信して、動作する
の流れになると理解しています。
このあたり、訂正等あればぜひご教授お願いします(´・ω・｀)
"rqt_graph"でノードを監視してみると、launch起動時は"joint_trajectory_controller"ノードがロードされているのが分かり
C++起動時には、Action Client/Serverが動作し、trajectory_msgが送信されていることがわかります。
C++のプログラムは
- gazebo_ros2_control/gazebo_ros2_control_demos/example/example_position.cpp line107 to line134
を以下のように書き換えます。
こちらは、使用するロボットのjoint_namesに応じて変更してください。
example_position.cpp
std::vector<std::string> joint_names = {
"rev_link_Rtop_roll1", "rev_link_Rtop_pitch", "rev_link_Rtop_roll2",
"rev_link_Rmid_roll1", "rev_link_Rmid_pitch", "rev_link_Rmid_roll2",
"rev_link_Rbtm_roll1", "rev_link_Rbtm_pitch", "rev_link_Rbtm_roll2",
"rev_link_Ltop_roll1", "rev_link_Ltop_pitch", "rev_link_Ltop_roll2",
"rev_link_Lmid_roll1", "rev_link_Lmid_pitch", "rev_link_Lmid_roll2",
"rev_link_Lbtm_roll1", "rev_link_Lbtm_pitch", "rev_link_Lbtm_roll2",
"rev_link_head"
};
std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
trajectory_msgs::msg::JointTrajectoryPoint point;
point.time_from_start = rclcpp::Duration::from_seconds(0.0);
point.positions.resize(joint_names.size());
for (size_t i=0; i != joint_names.size()/3; i++){
if (i<=2){
point.positions[3*i] = 0.0;
point.positions[3*i+1] = 0.0;
point.positions[3*i+2] = 0.0;
}
else{
point.positions[3*i] = 0.0;
point.positions[3*i+1] = 0.0;
point.positions[3*i+2] = 0.0;
}
}
trajectory_msgs::msg::JointTrajectoryPoint point1;
point1.time_from_start = rclcpp::Duration::from_seconds(1.0);
point1.positions.resize(joint_names.size());
for (size_t i=0; i != joint_names.size()/3; i++){
if (i<=2){
point1.positions[3*i] = -1.0;
point1.positions[3*i+1] = 1.0;
point1.positions[3*i+2] = 1.0;
}
else{
point1.positions[3*i] = 1.0;
point1.positions[3*i+1] = -1.0;
point1.positions[3*i+2] = -1.0;
}
}
trajectory_msgs::msg::JointTrajectoryPoint point2;
point2.time_from_start = rclcpp::Duration::from_seconds(2.0);
point2.positions.resize(joint_names.size());
for (size_t j=0; j != joint_names.size()/3; j++){
if (j<=2){
point2.positions[3*j] = 1.0;
point2.positions[3*j+1] = 1.0;
point2.positions[3*j+2] = 1.0;
}
else{
point2.positions[3*j] = -1.0;
point2.positions[3*j+1] = -1.0;
point2.positions[3*j+2] = -1.0;
}
}
trajectory_msgs::msg::JointTrajectoryPoint point3;
point3.time_from_start = rclcpp::Duration::from_seconds(3.0);
point3.positions.resize(joint_names.size());
for (size_t i=0; i != joint_names.size()/3; i++){
if (i<=2){
point3.positions[3*i] = 0.0;
point3.positions[3*i+1] = 0.0;
point3.positions[3*i+2] = 0.0;
}
else{
point3.positions[3*i] = 0.0;
point3.positions[3*i+1] = 0.0;
point3.positions[3*i+2] = 0.0;
}
}
points.push_back(point);
points.push_back(point1);
points.push_back(point2);
points.push_back(point3);
C++のROS2ノードを作成したので、CMakeLists.txtにも、依存関係等を追加します。
CMakeLists.txt
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(control_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(std_msgs REQUIRED)
add_executable(example_position src/example_position.cpp)
ament_target_dependencies(example_position
rclcpp
rclcpp_action
control_msgs
)
## Install
install(
TARGETS
example_position
DESTINATION
lib/${PROJECT_NAME}
)
ビルドが通せたら、実際にROS2とGazeboでロボットを動かしてみましょう。
First Shell
ros2 launch manta_v2_gazebo_ros2_control trajectory_controller.launch.py
Second Shell
ros2 run manta_v2_gazebo_ros2_control example_position
- 初期位置 > 振り上げ動作 > 振り下げ動作
と動作していることが確認できます。
action, client形式で通信しているので、指定した動作が終了すると、ロボットの動作も終了します。
ループさせる場合は、無理やりWhileループに入れるか(非推奨)、topic形式でプログラムを書き直せば良いかと。
Rviz2とGazeboを連携する
Gazeboでロボットを動かせるなら、Rviz2とも連携したくなったので、こちらも記載しておきます。
私は、xxxxx_bringupで使用しているローンチをros2_control対応させて実現させました。
xxxxx_bringup/launch下にxxxxx_ros2_control.launch.pyを作成し、以下を記述します。
manta_ros2_control.launch.py
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
packages_name = "manta_v2_description"
gz_pacakges_name = "manta_v2_gazebo_ros2_control"
xacro_file_name = "manta_v2.xacro"
rviz_file_name = "urdf_with_imu.rviz"
# Get URDF via xacro
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(packages_name), "urdf", xacro_file_name]
),
]
)
robot_description = {"robot_description": robot_description_content}
rviz_config_file = PathJoinSubstitution(
[FindPackageShare(packages_name), "rviz", rviz_file_name]
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
)
# ros2_control
robot_controllers = PathJoinSubstitution(
[
FindPackageShare(gz_pacakges_name),
"config",
"manta_v2_trajectory_controller.yaml",
]
)
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, robot_controllers],
output={
"stdout": "screen",
"stderr": "screen",
},
)
joint_state_controller_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=["joint_state_controller", "-c", "/controller_manager"],
)
joint_trajectory_controller_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=["joint_trajectory_controller", "-c", "/controller_manager"],
)
nodes = [
rviz_node,
ros2_control_node,
joint_state_controller_spawner,
joint_trajectory_controller_spawner,
]
return LaunchDescription(nodes)
ビルドを通したら、以下で確認してみましょう。
First Shell
ros2 launch manta_v2_gazebo_ros2_control trajectory_controller.launch.py
Second Shell
ros2 launch manta_v2_bringup manta_ros2_control.launch.py
Third Shell
ros2 run manta_v2_gazebo_ros2_control example_position
Rviz2側で反映されない場合は、一度ローンチを止め、再び起動してみてください。
また、Gazebo側を最初にローンチしましょう。
動いていることが確認できますね。
シミュレーション環境でここまで準備できれば、実機がなくともロボットの制御を確認することができますね。
以上、この記事が皆様のお役に立つことを願っています。
また、いいね！を頂けると非常に励みになります。
今後とも、どうぞよろしくお願いします。
Discussion