sciurus17_examplesのROS 2対応 (#144)

* allowed_start_tolerance設定

* gripper_controlサンプル追加

* neck_group, waist_group追加

* neck_controlサンプル追加

* waist_controlサンプル追加

* スタイル修正

* Gazebo上のcube位置変更

* pick_and_place_right_arm_waistサンプル追加

* 手先位置姿勢生成ライブラリ追加

* スタイル修正

* ポーズプリセットの追加

* ライブラリの更新に対応

* pick_and_place_left_armサンプル追加

* pick_and_place_two_armサンプル追加

* Revert "pick_and_place_two_armサンプル追加"

This reverts commit 51522f9.

* 不要なclassの削除

* 把持位置の変数化

* 変数名変更

* 変数名変更

* コメント修正

* コメント追加

* 駆動速度調整

* コメント追加

* 引数の定数化

* 不要なライブラリのincludeを削除

* シミュレータ動作が思いためmax_step_sizeを調整

* 把持位置調整

* ヘッダファイルのインクルード箇所を変更

* コメント修正

* グリッパ開閉角の定数化

sciurus17_examples/CMakeLists.txt

@@ -17,16 +17,61 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(angles REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 
+set(LIBRARY_NAME "pose_presets")
+add_library(${LIBRARY_NAME}
+  SHARED
+  src/${LIBRARY_NAME}.cpp
+)
+target_include_directories(
+  ${LIBRARY_NAME}
+  PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+ament_target_dependencies(
+  ${LIBRARY_NAME}
+  angles
+  geometry_msgs
+  tf2_geometry_msgs
+)
+
+ament_export_targets(export_${LIBRARY_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+  angles
+  geometry_msgs
+  tf2_geometry_msgs
+)
+install(
+  DIRECTORY include/
+  DESTINATION include/${LIBRARY_NAME}
+)
+
+install(
+  TARGETS ${LIBRARY_NAME}
+  EXPORT export_${LIBRARY_NAME}
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
 # Build and install node executables
 set(executable_list
+  gripper_control
+  neck_control
+  waist_control
+  pick_and_place_right_arm_waist
+  pick_and_place_left_arm
 )
 foreach(loop_var IN LISTS executable_list)
   add_executable(${loop_var} src/${loop_var}.cpp)
+  target_link_libraries(${loop_var} ${LIBRARY_NAME})
   ament_target_dependencies(${loop_var}
     geometry_msgs
     moveit_ros_planning_interface

sciurus17_examples/include/pose_presets.hpp

@@ -0,0 +1,32 @@
+// Copyright 2023 RT Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef POSE_PRESETS_HPP_
+#define POSE_PRESETS_HPP_
+
+#include "geometry_msgs/msg/pose.hpp"
+
+namespace pose_presets
+{
+// Pose型の位置姿勢を作成
+geometry_msgs::msg::Pose generate_pose(
+  const double x, const double y, const double z,
+  const double roll, const double pitch, const double yaw);
+// 右グリッパを下に向ける姿勢を作成
+geometry_msgs::msg::Pose right_arm_downward(const double x, const double y, const double z);
+// 左グリッパを下に向ける姿勢を作成
+geometry_msgs::msg::Pose left_arm_downward(const double x, const double y, const double z);
+}  // namespace pose_presets
+
+#endif  // POSE_PRESETS_HPP_

sciurus17_examples/launch/example.launch.py

@@ -0,0 +1,84 @@
+# Copyright 2023 RT Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from sciurus17_description.robot_description_loader import RobotDescriptionLoader
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch_ros.actions import SetParameter
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+import yaml
+
+
+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_loader = RobotDescriptionLoader()
+
+    robot_description_semantic_config = load_file(
+        'sciurus17_moveit_config', 'config/sciurus17.srdf')
+    robot_description_semantic = {
+        'robot_description_semantic': robot_description_semantic_config}
+
+    kinematics_yaml = load_yaml('sciurus17_moveit_config', 'config/kinematics.yaml')
+
+    declare_example_name = DeclareLaunchArgument(
+        'example', default_value='gripper_control',
+        description=('Set an example executable name: '
+                     '[gripper_control, neck_control, waist_control,'
+                     'pick_and_place_right_arm_waist, pick_and_place_left_arm]')
+    )
+
+    declare_use_sim_time = DeclareLaunchArgument(
+        'use_sim_time', default_value='false',
+        description=('Set true when using the gazebo simulator.')
+    )
+
+    example_node = Node(name=[LaunchConfiguration('example'), '_node'],
+                        package='sciurus17_examples',
+                        executable=LaunchConfiguration('example'),
+                        output='screen',
+                        parameters=[{'robot_description': description_loader.load()},
+                                    robot_description_semantic,
+                                    kinematics_yaml])
+
+    return LaunchDescription([
+        declare_use_sim_time,
+        SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')),
+        declare_example_name,
+        example_node
+    ])

sciurus17_examples/src/gripper_control.cpp

@@ -0,0 +1,99 @@
+// Copyright 2023 RT Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Reference:
+// https://github.com/ros-planning/moveit2_tutorials/blob/humble/doc/
+// examples/move_group_interface/src/move_group_interface_tutorial.cpp
+
+#include <cmath>
+
+#include "angles/angles.h"
+#include "moveit/move_group_interface/move_group_interface.h"
+#include "rclcpp/rclcpp.hpp"
+
+using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface;
+
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("gripper_control");
+
+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_arm_node =
+    rclcpp::Node::make_shared("move_group_arm_node", node_options);
+  auto move_group_r_gripper_node =
+    rclcpp::Node::make_shared("move_group_r_gripper_node", node_options);
+  auto move_group_l_gripper_node =
+    rclcpp::Node::make_shared("move_group_l_gripper_node", node_options);
+  // For current state monitor
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(move_group_arm_node);
+  executor.add_node(move_group_r_gripper_node);
+  executor.add_node(move_group_l_gripper_node);
+  std::thread([&executor]() {executor.spin();}).detach();
+
+  // 両腕制御用MoveGroupInterface
+  MoveGroupInterface move_group_arm(move_group_arm_node, "two_arm_group");
+  // 駆動速度の調整
+  move_group_arm.setMaxVelocityScalingFactor(0.1);  // Set 0.0 ~ 1.0
+  move_group_arm.setMaxAccelerationScalingFactor(0.1);  // Set 0.0 ~ 1.0
+
+  // 右グリッパ制御用MoveGroupInterface
+  MoveGroupInterface move_group_r_gripper(move_group_r_gripper_node, "r_gripper_group");
+  // 駆動速度の調整
+  move_group_r_gripper.setMaxVelocityScalingFactor(1.0);  // Set 0.0 ~ 1.0
+  move_group_r_gripper.setMaxAccelerationScalingFactor(1.0);  // Set 0.0 ~ 1.0
+  auto r_gripper_joint_values = move_group_r_gripper.getCurrentJointValues();
+  const double R_GRIPPER_CLOSE = 0.0;
+  const double R_GRIPPER_OPEN = angles::from_degrees(40.0);
+
+  // 左グリッパ制御用MoveGroupInterface
+  MoveGroupInterface move_group_l_gripper(move_group_l_gripper_node, "l_gripper_group");
+  // 駆動速度の調整
+  move_group_l_gripper.setMaxVelocityScalingFactor(1.0);  // Set 0.0 ~ 1.0
+  move_group_l_gripper.setMaxAccelerationScalingFactor(1.0);  // Set 0.0 ~ 1.0
+  auto l_gripper_joint_values = move_group_l_gripper.getCurrentJointValues();
+  const double L_GRIPPER_CLOSE = 0.0;
+  const double L_GRIPPER_OPEN = angles::from_degrees(-40.0);
+
+  // SRDFに定義されている"two_arm_init_pose"の姿勢にする
+  move_group_arm.setNamedTarget("two_arm_init_pose");
+  move_group_arm.move();
+
+  // 右グリッパ開閉
+  for (int i = 0; i < 2; i++) {
+    r_gripper_joint_values[0] = R_GRIPPER_OPEN;
+    move_group_r_gripper.setJointValueTarget(r_gripper_joint_values);
+    move_group_r_gripper.move();
+
+    r_gripper_joint_values[0] = R_GRIPPER_CLOSE;
+    move_group_r_gripper.setJointValueTarget(r_gripper_joint_values);
+    move_group_r_gripper.move();
+  }
+
+  // 左グリッパ開閉
+  for (int i = 0; i < 2; i++) {
+    l_gripper_joint_values[0] = L_GRIPPER_OPEN;
+    move_group_l_gripper.setJointValueTarget(l_gripper_joint_values);
+    move_group_l_gripper.move();
+
+    l_gripper_joint_values[0] = L_GRIPPER_CLOSE;
+    move_group_l_gripper.setJointValueTarget(l_gripper_joint_values);
+    move_group_l_gripper.move();
+  }
+
+  rclcpp::shutdown();
+  return 0;
+}

sciurus17_examples/src/neck_control.cpp

@@ -0,0 +1,82 @@
+// Copyright 2023 RT Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Reference:
+// https://github.com/ros-planning/moveit2_tutorials/blob/humble/doc/
+// examples/move_group_interface/src/move_group_interface_tutorial.cpp
+
+#include "angles/angles.h"
+#include "moveit/move_group_interface/move_group_interface.h"
+#include "rclcpp/rclcpp.hpp"
+
+using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface;
+
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("neck_control");
+
+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("neck_control", node_options);
+  // For current state monitor
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(move_group_node);
+  std::thread([&executor]() {executor.spin();}).detach();
+
+  // 首制御用MoveGroupInterface
+  MoveGroupInterface move_group_neck(move_group_node, "neck_group");
+  // 駆動速度の調整
+  move_group_neck.setMaxVelocityScalingFactor(0.1);  // Set 0.0 ~ 1.0
+  move_group_neck.setMaxAccelerationScalingFactor(0.1);  // Set 0.0 ~ 1.0
+
+  // SRDFに定義されている"neck_init_pose"の姿勢にする
+  move_group_neck.setNamedTarget("neck_init_pose");
+  move_group_neck.move();
+
+  // 現在角度をベースに、目標角度を作成する
+  auto joint_values = move_group_neck.getCurrentJointValues();
+
+  // 首を左に向ける
+  joint_values[0] = angles::from_degrees(45.0);
+  move_group_neck.setJointValueTarget(joint_values);
+  move_group_neck.move();
+
+  // 首を右に向ける
+  joint_values[0] = angles::from_degrees(-45.0);
+  move_group_neck.setJointValueTarget(joint_values);
+  move_group_neck.move();
+
+  // 首を前に向ける
+  joint_values[0] = angles::from_degrees(0.0);
+  move_group_neck.setJointValueTarget(joint_values);
+  move_group_neck.move();
+
+  // 首を上に向ける
+  joint_values[1] = angles::from_degrees(45.0);
+  move_group_neck.setJointValueTarget(joint_values);
+  move_group_neck.move();
+
+  // 首を下に向ける
+  joint_values[1] = angles::from_degrees(-45.0);
+  move_group_neck.setJointValueTarget(joint_values);
+  move_group_neck.move();
+
+  // "neck_init_pose"に戻す
+  move_group_neck.setNamedTarget("neck_init_pose");
+  move_group_neck.move();
+
+  rclcpp::shutdown();
+  return 0;
+}