🐙
ROS2: Move Group C++ Interfaceでロボットを動かす
海洋ロボコンをやってた人です。
今回は、Moveitが提供するMove Group C++ Interfaceでオリジナルのロボットを動かす方法を引き継ぎも兼ねて記述しておきます。
これからMoveit2で自分の研究機体を制御したい方のお力添えになれば幸いです。
ROS2: Move Group C++ Interfaceでロボットの動かす
この記事を読むことで、以下のようにC++プログラムでmove groupを使用した制御を行えるようになります。
使用するモデルは、アイコンでも表示されているマンタ型ロボットで紹介していきます。
なお、ロボットのハード設計からソフト設計まで全て私が担当しており、所属研究室の教授からも承諾を得ています。
また、ROS2でMoveit2のパッケージを作る方法も以下にまとめているので、moveitパッケージをまだ作成していない方は、こちらも合わせてご覧頂けると良いと思います。
※2023/02/26追記: 本記事の「ROS2」表記、正しくは「ROS 2」です。
move group c++ interface用のパッケージを作成する
まずはmove group c++ interface用のパッケージをワークスペース内へ作成します。
cd manta_ws/src
ros2 pkg create --build-type ament_cmake manta_v2_run_move_group
他社のGithubを見ていると、xxxxx_run_move_groupというパッケージ名をつけている方がいたので、今回は上記のパッケージ名で作成をしました。
続いて、必要なプログラムを作成していきます。
touch manta_v2_run_move_group/src/run_move_group.cpp
touch manta_v2_run_move_group/launch/move_group.launch.py
touch manta_v2_run_move_group/launch/move_group_interface.launch.py
まずはメインのrun_move_group.cppのプログラムを記述していきます。
run_move_group.cpp
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/msg/display_robot_state.hpp>
#include <moveit_msgs/msg/display_trajectory.hpp>
#include <moveit_msgs/msg/attached_collision_object.hpp>
#include <moveit_msgs/msg/collision_object.hpp>
#include <moveit/macros/console_colors.h>
static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_demo");
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto move_group_node = rclcpp::Node::make_shared("move_group_interface", node_options);
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(move_group_node);
std::thread([&executor]() { executor.spin(); }).detach();
static const std::string PLANNING_GROUP = "Manta";
static const std::string PLANNING_GROUP_LTOP = "Ltop";
moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP);
moveit::planning_interface::MoveGroupInterface move_group_ltop(move_group_node, PLANNING_GROUP_LTOP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const moveit::core::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
const moveit::core::JointModelGroup* ltop_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP_LTOP);
// RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 0.5;
RCLCPP_INFO(LOGGER, "Planning frame: %s", move_group.getPlanningFrame().c_str());
RCLCPP_INFO(LOGGER, "End effector link: %s", move_group.getEndEffectorLink().c_str());
RCLCPP_INFO(LOGGER, "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
RCLCPP_INFO(LOGGER, "Joint Names:");
std::copy(move_group.getJointNames().begin(), move_group.getJointNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
moveit::planning_interface::MoveGroupInterface::Plan my_plan_ltop;
bool success;
// Motion1 - joint group
std::vector<double> joint_group_positions;
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
for (size_t i=0; i != joint_group_positions.size()/3; i++){
if (i<=2){
joint_group_positions[3*i] = -0.2;
joint_group_positions[3*i+1] = 0.2;
joint_group_positions[3*i+2] = 0.2;
}
else{
joint_group_positions[3*i] = 0.2;
joint_group_positions[3*i+1] = -0.2;
joint_group_positions[3*i+2] = -0.2;
}
}
move_group.setJointValueTarget(joint_group_positions);
move_group.setPlanningTime(10.0);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
move_group.move();
move_group.clearPathConstraints();
// Motion2 - ltop group
std::vector<double> ltop_group_positions;
moveit::core::RobotStatePtr ltop_current_state = move_group_ltop.getCurrentState();
ltop_current_state->copyJointGroupPositions(ltop_model_group, ltop_group_positions);
ltop_group_positions[0] += 0.5;
move_group_ltop.setJointValueTarget(ltop_group_positions);
move_group_ltop.setPlanningTime(20.0);
success = (move_group_ltop.plan(my_plan_ltop) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
move_group_ltop.move();
move_group_ltop.clearPathConstraints();
// Motion3 - group state
move_group.setStartState(*move_group.getCurrentState());
move_group.setJointValueTarget(move_group.getNamedTargetValues("Init"));
move_group.setPlanningTime(10.0);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
move_group.move();
move_group.clearPathConstraints();
// Motion4 - group state
move_group.setStartState(*move_group.getCurrentState());
move_group.setJointValueTarget(move_group.getNamedTargetValues("Up"));
move_group.setPlanningTime(10.0);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
move_group.move();
move_group.clearPathConstraints();
// Motion5 - group state
move_group.setStartState(*move_group.getCurrentState());
move_group.setJointValueTarget(move_group.getNamedTargetValues("Init"));
move_group.setPlanningTime(10.0);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
move_group.move();
move_group.clearPathConstraints();
rclcpp::shutdown();
return 0;
}
MoveIt!はロボットのモーションプランニング、マニピュレーション、運動学、逆運動学などのプランニング機能を提供しているプランニングフレームワークです。
MoveIt!が提供しているmove_groupノードを利用することで、一連のROSアクション、サービスを利用することができます。
下図はイメージ
本記事では、MoveItのtutorialとして一番始めに紹介されるMove Group C++ Interfaceを使用してプログラムを準備しました。
以下に、ざっくりと内容の説明も記載していきます。
まず、PLANNING_GROUPを定義し、move groupのジョイントグループを取得する記述をします。
PLANNING_GROUPはxxxxx.srdfで定義しているgroup name=yyyを示すので、間違えないように注意しましょう。
自分はcontroller名とmove group nameを違う名前で定義していたので、ここで一回つまづきました。
run_move_group.cpp
static const std::string PLANNING_GROUP = "Manta";
static const std::string PLANNING_GROUP_LTOP = "Ltop";
moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP);
moveit::planning_interface::MoveGroupInterface move_group_ltop(move_group_node, PLANNING_GROUP_LTOP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const moveit::core::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
const moveit::core::JointModelGroup* ltop_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP_LTOP);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
moveit::planning_interface::MoveGroupInterface::Plan my_plan_ltop;
また、各move groupのジョイント名を知りたかったので、Tutorialプログラムに以下を追加し、各move groupのもつジョイント名をデバッグ表示できるようにしています。
run_move_group.cpp
RCLCPP_INFO(LOGGER, "Joint Names:");
std::copy(move_group.getJointNames().begin(), move_group.getJointNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
Motion1 - joint group
ここからは、各move groupのモーションについて説明していきます。
run_move_group.cpp
std::vector<double> joint_group_positions;
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
for (size_t i=0; i != joint_group_positions.size()/3; i++){
if (i<=2){
joint_group_positions[3*i] = -0.2;
joint_group_positions[3*i+1] = 0.2;
joint_group_positions[3*i+2] = 0.2;
}
else{
joint_group_positions[3*i] = 0.2;
joint_group_positions[3*i+1] = -0.2;
joint_group_positions[3*i+2] = -0.2;
}
}
move_group.setJointValueTarget(joint_group_positions);
move_group.setPlanningTime(10.0);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
move_group.move();
move_group.clearPathConstraints();
ベクターでjoint_group_positionsと定義し、現在のジョイント情報をコピーします。
その後、joint_group_positionsに指令値を与えsetJointValueTargetで値を設定します。
setPlanningTimeはプランニングを計画するための時間で、設定したプランナーが成功するために確保時間を増やす場合があります。
ここではプランニング計画の確保時間を10秒としています。
success以降で、プランニングの計画が成功したら、実際に動かす処理を記述しています。
Motion2 - ltop group
Motion1では、group name = "Manta"を用いて、プランニングを行っていましたが、Motion2では、group name = "Ltop"を用いて、プランニングしてみます。
産業用多関節ロボット、協働ロボットでは、move_groupを
-
arm_controller
-
gripper_controller
と記述することが多いため、異なるmove groupのプランニング方法も記載することにしました。
run_move_group.cpp
std::vector<double> ltop_group_positions;
moveit::core::RobotStatePtr ltop_current_state = move_group_ltop.getCurrentState();
ltop_current_state->copyJointGroupPositions(ltop_model_group, ltop_group_positions);
ltop_group_positions[0] += 0.5;
move_group_ltop.setJointValueTarget(ltop_group_positions);
move_group_ltop.setPlanningTime(20.0);
success = (move_group_ltop.plan(my_plan_ltop) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
move_group_ltop.move();
move_group_ltop.clearPathConstraints();
記述内容は、Motion1とさほど変わらず、はじめに定義したmove_group_ltopを使ってsetJoint, setPlanning, moveを行っているだけです。
Motion3 - 5 NamedTargetValues
最後にMotion3〜5を説明します。
move group interfaceには、xxx.srdfで指定したgroup_stateをgetNamedTargetValuesで呼び出すことができます。
run_move_group.cpp
move_group.setStartState(*move_group.getCurrentState());
move_group.setJointValueTarget(move_group.getNamedTargetValues("Up"));
move_group.setPlanningTime(10.0);
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
move_group.move();
move_group.clearPathConstraints();
この機能を使うことで、事前に登録してあるモーションへ自在に動かすことができるので、モーションの確認などには最適かもしれません。
launch等を記述しビルドする
ここまでできたら、launch, CMakeLists等を記述していきましょう。
move group c++ interfaceのデモンストレーションでは
- move_group.launch.pyでmove_group_nodeを起動
- move_group_interface.launch.pyでrun_move_group.cppのROS2ノードを起動
して動作を確認しているので、こちらに沿ってプログラムを記載していきます。
また、folder, fileの名前は適宜変更してください。
move_group.launch.py
import os
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory
import xacro
def load_file(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return file.read()
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return yaml.safe_load(file)
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def generate_launch_description():
description_folder = "manta_v2_description"
moveit_config_folder = "manta_v2_moveit_config"
xacro_file = "urdf/manta_v2.xacro"
srdf_file = "config/manta_v2.srdf"
# planning_context
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory(description_folder),
xacro_file,
)
)
robot_description = {"robot_description": robot_description_config.toxml()}
robot_description_semantic_config = load_file(
moveit_config_folder, srdf_file
)
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
kinematics_yaml = load_yaml(
moveit_config_folder, "config/kinematics.yaml"
)
robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}
# Planning Functionality
ompl_planning_pipeline_config = {
"move_group": {
"planning_plugin": "ompl_interface/OMPLPlanner",
"request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"start_state_max_bounds_error": 0.1,
}
}
ompl_planning_yaml = load_yaml(
moveit_config_folder, "config/ompl_planning.yaml"
)
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
# Trajectory Execution Functionality
moveit_simple_controllers_yaml = load_yaml(
moveit_config_folder, "config/manta_v2_controllers.yaml"
)
moveit_controllers = {
"moveit_simple_controller_manager": moveit_simple_controllers_yaml,
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
}
trajectory_execution = {
"moveit_manage_controllers": True,
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
"trajectory_execution.allowed_goal_duration_margin": 0.5,
"trajectory_execution.allowed_start_tolerance": 0.01,
}
planning_scene_monitor_parameters = {
"publish_planning_scene": True,
"publish_geometry_updates": True,
"publish_state_updates": True,
"publish_transforms_updates": True,
}
# Start the actual move_group node/action server
run_move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
robot_description,
robot_description_semantic,
kinematics_yaml,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
],
)
# RViz
rviz_config_file = (
get_package_share_directory("manta_v2_run_move_group") + "/launch/move_group.rviz"
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
parameters=[
robot_description,
robot_description_semantic,
ompl_planning_pipeline_config,
kinematics_yaml,
],
)
# Static TF
static_tf = Node(
package="tf2_ros",
executable="static_transform_publisher",
name="static_transform_publisher",
output="log",
arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_link"],
)
# Publish TF
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
output="both",
parameters=[robot_description],
)
# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory(moveit_config_folder),
"config",
"manta_v2_ros_controllers.yaml",
)
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, ros2_controllers_path],
output={
"stdout": "screen",
"stderr": "screen",
},
)
# Load controllers
load_controllers = []
for controller in [
"rtop_controller",
"rmid_controller",
"rbtm_controller",
"ltop_controller",
"lmid_controller",
"lbtm_controller",
"manta_controller",
"joint_state_broadcaster",
]:
load_controllers += [
ExecuteProcess(
cmd=["ros2 run controller_manager spawner.py {}".format(controller)],
shell=True,
output="screen",
)
]
# Warehouse mongodb server
mongodb_server_node = Node(
package="warehouse_ros_mongo",
executable="mongo_wrapper_ros.py",
parameters=[
{"warehouse_port": 33829},
{"warehouse_host": "localhost"},
{"warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection"},
],
output="screen",
)
return LaunchDescription(
[
rviz_node,
static_tf,
robot_state_publisher,
run_move_group_node,
ros2_control_node,
# mongodb_server_node,
]
+ load_controllers
)
続いて、cppのROS2ノードを起動するlaunchファイルを記述
move_group_interface.launch.py
import os
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import xacro
def load_file(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return file.read()
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return yaml.safe_load(file)
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def generate_launch_description():
description_folder = "manta_v2_description"
moveit_config_folder = "manta_v2_moveit_config"
xacro_file = "urdf/manta_v2.xacro"
srdf_file = "config/manta_v2.srdf"
rviz_file = "rviz/run_move_group.rviz"
# planning_context
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory(description_folder),
xacro_file,
)
)
robot_description = {"robot_description": robot_description_config.toxml()}
robot_description_semantic_config = load_file(
moveit_config_folder, srdf_file
)
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
kinematics_yaml = load_yaml(
moveit_config_folder, "config/kinematics.yaml"
)
# MoveGroupInterface demo executable
move_group_demo = Node(
name="manta_v2_run_move_group",
package="manta_v2_run_move_group",
executable="manta_v2_run_move_group",
output="screen",
parameters=[
robot_description,
robot_description_semantic,
kinematics_yaml],
)
return LaunchDescription([move_group_demo])
cppをadd_executableへ追加するために、CMakeLists.txtも編集
cmake_minimum_required(VERSION 3.5)
project(manta_v2_run_move_group)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(moveit_common REQUIRED)
find_package(ament_cmake REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(rosidl_default_runtime REQUIRED)
find_package(Boost REQUIRED COMPONENTS system)
add_executable(manta_v2_run_move_group src/run_move_group.cpp)
ament_target_dependencies(manta_v2_run_move_group
moveit_ros_planning_interface
Boost
)
install(TARGETS manta_v2_run_move_group
EXPORT export_${PROJECT_NAME}
DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY
launch
DESTINATION share/${PROJECT_NAME}
)
ament_package()
ここまでできたら、ビルドしてみましょう。
colcon build --packages-select manta_v2_run_move_group
ビルドできればOKですね。
.rvizファイルはxxxxx_moveit_config内の.rvizファイルを使い回しでもOKです。
自分はmoveit_config内のrvizファイルをコピーして、名前を変更したものを使ってます。
move groupの動作テストする
では早速、確認してみましょう。
First Shell
. install/setup.bash
ros2 launch manta_v2_run_move_group move_group.launch.py
Second Shell
. install/setup.bash
ros2 launch manta_v2_run_move_group move_group_interface.launch.py
Motion1の動作
Motion2の動作
Motion3〜5の動作
Rviz2, Gazebo, Hardwareの3つで連携できていることが確認できます。
Reference
moveit::planning_interface::MoveGroup Class Reference
ROS Tutorial: Pick and Place task with the Moveit C++ interface
以上、move group C++ interfaceでロボットを動かす方法でした。
この記事が少しでも、皆様のお役に立てば幸いです。
また、いいねをいただけますと、非常にモチベーションに繋がります(´・ω・｀)
Discussion