レビュー指摘事項に対応

rt-net · Dec 18, 2024 · 67ed12e · 67ed12e
1 parent 896b049
commit 67ed12e
Show file tree

Hide file tree

Showing 7 changed files with 81 additions and 114 deletions.
diff --git a/sciurus17_examples_py/README.md b/sciurus17_examples_py/README.md
@@ -2,15 +2,8 @@
 
 このパッケージはSciurus17 ROS 2パッケージのPythonサンプルコード集です。
 
-- [sciurus17\_examples\_py](#sciurus17_examples_py)
-  - [準備（実機を使う場合）](#準備実機を使う場合)
-    - [1. Sciurus17本体をPCに接続する](#1-sciurus17本体をpcに接続する)
-    - [2. USB通信ポートの接続を確認する](#2-usb通信ポートの接続を確認する)
-    - [3. move\_groupとcontrollerを起動する](#3-move_groupとcontrollerを起動する)
-  - [準備 (Gazeboを使う場合)](#準備-gazeboを使う場合)
-    - [1. move\_groupとGazeboを起動する](#1-move_groupとgazeboを起動する)
-  - [準備（Mock Componentsを使う場合）](#準備mock-componentsを使う場合)
-    - [1. move\_groupとcontrollerを起動する](#1-move_groupとcontrollerを起動する)
+- [sciurus17\_examples](#sciurus17_examples)
+  - [起動方法](#起動方法)
   - [サンプルプログラムを実行する](#サンプルプログラムを実行する)
     - [Gazeboでサンプルプログラムを実行する場合](#gazeboでサンプルプログラムを実行する場合)
   - [Examples](#examples)
@@ -20,62 +13,8 @@
     - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist)
     - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm)
 
-
-## 準備（実機を使う場合）
-
-### 1. Sciurus17本体をPCに接続する
-Sciurus17本体をPCに接続します。
-接続方法は製品マニュアルを参照してください。
-
-**※Sciurus17本体が接触しないように、十分なスペースを確保してください。**
-
-### 2. USB通信ポートの接続を確認する
-
-USB通信ポートの設定については`sciurus17_control`の
-[README](../sciurus17_control/README.md)
-を参照してください。
-
-**正しく設定できていない場合、Sciurus17が動作しないので注意してください**
-
-### 3. move_groupとcontrollerを起動する
-
-次のコマンドでmove_group (`sciurus17_moveit_config`)と
-controller (`sciurus17_control`)を起動します。
-
-```sh
-ros2 launch sciurus17_examples demo.launch.py
-```
-
-## 準備 (Gazeboを使う場合)
-
-### 1. move_groupとGazeboを起動する
-
-次のコマンドでmove_group (`sciurus17_moveit_config`)と
-Gazeboを起動します。
-
-```sh
-ros2 launch sciurus17_gazebo sciurus17_with_table.launch.py
-```
-
-頭部カメラや胸部カメラのシミュレーションを行わない場合は、
-`use_head_camera`、`use_chest_camera`オプションを`false`に設定します。
-
-```sh
-ros2 launch sciurus17_gazebo sciurus17_with_table.launch.py use_head_camera:=false use_chest_camera:=false
-```
-
-## 準備（Mock Componentsを使う場合）
-
-### 1. move_groupとcontrollerを起動する
-
-次のコマンドでmove_group (`sciurus17_moveit_config`)と
-controller (`sciurus17_control`)を起動します。
-
-```sh
-ros2 launch sciurus17_examples demo.launch.py use_mock_components:=true
-```
-
-Mock Componentsではカメラを使ったサンプルを実行することはできません。
+## 起動方法
+Sciurus17の起動方法は[sciurus17_examplesのREADME](../sciurus17_examples/README.md)を参照してください。
 
 ## サンプルプログラムを実行する
 

diff --git a/sciurus17_examples_py/launch/example.launch.py b/sciurus17_examples_py/launch/example.launch.py
@@ -17,28 +17,25 @@
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
+from launch_ros.actions import SetParameter
 from moveit_configs_utils import MoveItConfigsBuilder
 from sciurus17_description.robot_description_loader import RobotDescriptionLoader
 
 
 def generate_launch_description():
-    ld = LaunchDescription()
+
     description_loader = RobotDescriptionLoader()
-
-    ld.add_action(
-        DeclareLaunchArgument('loaded_description',
+    declare_loaded_description = DeclareLaunchArgument('loaded_description',
                               default_value=description_loader.load(),
                               description='Set robot_description text.  \
                         It is recommended to use RobotDescriptionLoader() \
-                        in sciurus17_description.'))
+                        in sciurus17_description.')
 
     moveit_config = (MoveItConfigsBuilder('sciurus17').planning_scene_monitor(
         publish_robot_description=True,
         publish_robot_description_semantic=True,
-    ).trajectory_execution(file_path='config/moveit_controllers.yaml').planning_pipelines(
-        pipelines=['ompl']).moveit_cpp(
-            file_path=get_package_share_directory('sciurus17_examples_py') +
-            '/config/sciurus17_moveit_py_examples.yaml').to_moveit_configs())
+    ).moveit_cpp(file_path=get_package_share_directory('sciurus17_examples_py') +
+        '/config/sciurus17_moveit_py_examples.yaml').to_moveit_configs())
 
     moveit_config.robot_description = {
         'robot_description': LaunchConfiguration('loaded_description')
@@ -53,6 +50,11 @@ def generate_launch_description():
                      '[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_py',
@@ -61,7 +63,10 @@ def generate_launch_description():
         parameters=[moveit_config.to_dict()],
     )
 
-    ld.add_action(declare_example_name)
-    ld.add_action(example_node)
-
-    return ld
+    return LaunchDescription([
+        declare_loaded_description,
+        declare_use_sim_time,
+        SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')),
+        declare_example_name,
+        example_node
+    ])
diff --git a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py
@@ -75,6 +75,7 @@ def main(args=None):
         single_plan_parameters=plan_request_params,
     )
 
+    # 右グリッパ開閉
     for _ in range(2):
         r_gripper.set_start_state_to_current_state()
         robot_state = RobotState(robot_model)
@@ -98,6 +99,7 @@ def main(args=None):
             single_plan_parameters=gripper_plan_request_params,
         )
 
+    # 左グリッパ開閉
     for _ in range(2):
         l_gripper.set_start_state_to_current_state()
         robot_state = RobotState(robot_model)

diff --git a/sciurus17_examples_py/sciurus17_examples_py/neck_control.py b/sciurus17_examples_py/sciurus17_examples_py/neck_control.py
@@ -65,6 +65,7 @@ def main(args=None):
         robot_state = scene.current_state
         joint_values = robot_state.get_joint_group_positions('neck_group')
 
+    # 首を左に向ける
     joint_values[0] = math.radians(45.0)
     neck.set_start_state_to_current_state()
     robot_state = RobotState(robot_model)
@@ -77,6 +78,7 @@ def main(args=None):
         single_plan_parameters=plan_request_params,
     )
 
+    # 首を右に向ける
     joint_values[0] = math.radians(-45.0)
     neck.set_start_state_to_current_state()
     robot_state = RobotState(robot_model)
@@ -89,6 +91,7 @@ def main(args=None):
         single_plan_parameters=plan_request_params,
     )
 
+    # 首を前に向ける
     joint_values[0] = math.radians(0.0)
     neck.set_start_state_to_current_state()
     robot_state = RobotState(robot_model)
@@ -101,6 +104,7 @@ def main(args=None):
         single_plan_parameters=plan_request_params,
     )
 
+    # 首を上に向ける
     joint_values[1] = math.radians(45.0)
     neck.set_start_state_to_current_state()
     robot_state = RobotState(robot_model)
@@ -113,6 +117,7 @@ def main(args=None):
         single_plan_parameters=plan_request_params,
     )
 
+    # 首を下に向ける
     joint_values[1] = math.radians(-45.0)
     neck.set_start_state_to_current_state()
     robot_state = RobotState(robot_model)
@@ -125,6 +130,7 @@ def main(args=None):
         single_plan_parameters=plan_request_params,
     )
 
+    # 'neck_init_pose'に戻す
     neck.set_start_state_to_current_state()
     neck.set_goal_state(configuration_name='neck_init_pose')
     plan_and_execute(

diff --git a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py
@@ -94,9 +94,10 @@ def main(args=None):
 
     # 物体の上に腕を伸ばす
     l_arm.set_start_state_to_current_state()
-    l_arm.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'),
-                                                      pose=PRE_AND_POST_GRASP_POSE),
-                         pose_link='l_link7')
+    goal_pose = PoseStamped()
+    goal_pose.header.frame_id='base_link'
+    goal_pose.pose = PRE_AND_POST_GRASP_POSE
+    l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7')
     plan_and_execute(
         sciurus17,
         l_arm,
@@ -106,9 +107,10 @@ def main(args=None):
 
     # 掴みに行く
     l_arm.set_start_state_to_current_state()
-    l_arm.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'),
-                                                      pose=GRASP_POSE),
-                         pose_link='l_link7')
+    goal_pose = PoseStamped()
+    goal_pose.header.frame_id='base_link'
+    goal_pose.pose = GRASP_POSE
+    l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7')
     plan_and_execute(
         sciurus17,
         l_arm,
@@ -130,9 +132,10 @@ def main(args=None):
 
     # 持ち上げる
     l_arm.set_start_state_to_current_state()
-    l_arm.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'),
-                                                      pose=PRE_AND_POST_GRASP_POSE),
-                         pose_link='l_link7')
+    goal_pose = PoseStamped()
+    goal_pose.header.frame_id='base_link'
+    goal_pose.pose = PRE_AND_POST_GRASP_POSE
+    l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7')
     plan_and_execute(
         sciurus17,
         l_arm,
@@ -142,9 +145,10 @@ def main(args=None):
 
     # 移動する
     l_arm.set_start_state_to_current_state()
-    l_arm.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'),
-                                                      pose=PRE_AND_POST_RELEASE_POSE),
-                         pose_link='l_link7')
+    goal_pose = PoseStamped()
+    goal_pose.header.frame_id='base_link'
+    goal_pose.pose = PRE_AND_POST_RELEASE_POSE
+    l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7')
     plan_and_execute(
         sciurus17,
         l_arm,
@@ -154,9 +158,10 @@ def main(args=None):
 
     # 下ろす
     l_arm.set_start_state_to_current_state()
-    l_arm.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'),
-                                                      pose=RELEASE_POSE),
-                         pose_link='l_link7')
+    goal_pose = PoseStamped()
+    goal_pose.header.frame_id='base_link'
+    goal_pose.pose = RELEASE_POSE
+    l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7')
     plan_and_execute(
         sciurus17,
         l_arm,
@@ -178,9 +183,10 @@ def main(args=None):
 
     # ハンドを持ち上げる
     l_arm.set_start_state_to_current_state()
-    l_arm.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'),
-                                                      pose=PRE_AND_POST_RELEASE_POSE),
-                         pose_link='l_link7')
+    goal_pose = PoseStamped()
+    goal_pose.header.frame_id='base_link'
+    goal_pose.pose = PRE_AND_POST_RELEASE_POSE
+    l_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7')
     plan_and_execute(
         sciurus17,
         l_arm,

diff --git a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_right_arm_waist.py
@@ -107,9 +107,10 @@ def main(args=None):
     # 物体の上に腕を伸ばす
     r_arm_waist.set_path_constraints(path_constraints=constraints)
     r_arm_waist.set_start_state_to_current_state()
-    r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'),
-                                                            pose=PRE_AND_POST_GRASP_POSE),
-                               pose_link='r_link7')
+    goal_pose = PoseStamped()
+    goal_pose.header.frame_id='base_link'
+    goal_pose.pose = PRE_AND_POST_GRASP_POSE
+    r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7')
     plan_and_execute(
         sciurus17,
         r_arm_waist,
@@ -119,9 +120,10 @@ def main(args=None):
 
     # 掴みに行く
     r_arm_waist.set_start_state_to_current_state()
-    r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'),
-                                                            pose=GRASP_POSE),
-                               pose_link='r_link7')
+    goal_pose = PoseStamped()
+    goal_pose.header.frame_id='base_link'
+    goal_pose.pose = GRASP_POSE
+    r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7')
     plan_and_execute(
         sciurus17,
         r_arm_waist,
@@ -143,9 +145,10 @@ def main(args=None):
 
     # 持ち上げる
     r_arm_waist.set_start_state_to_current_state()
-    r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'),
-                                                            pose=PRE_AND_POST_GRASP_POSE),
-                               pose_link='r_link7')
+    goal_pose = PoseStamped()
+    goal_pose.header.frame_id='base_link'
+    goal_pose.pose = PRE_AND_POST_GRASP_POSE
+    r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7')
     plan_and_execute(
         sciurus17,
         r_arm_waist,
@@ -155,9 +158,10 @@ def main(args=None):
 
     # 移動する
     r_arm_waist.set_start_state_to_current_state()
-    r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'),
-                                                            pose=PRE_AND_POST_RELEASE_POSE),
-                               pose_link='r_link7')
+    goal_pose = PoseStamped()
+    goal_pose.header.frame_id='base_link'
+    goal_pose.pose = PRE_AND_POST_RELEASE_POSE
+    r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7')
     plan_and_execute(
         sciurus17,
         r_arm_waist,
@@ -167,9 +171,10 @@ def main(args=None):
 
     # 下ろす
     r_arm_waist.set_start_state_to_current_state()
-    r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'),
-                                                            pose=RELEASE_POSE),
-                               pose_link='r_link7')
+    goal_pose = PoseStamped()
+    goal_pose.header.frame_id='base_link'
+    goal_pose.pose = RELEASE_POSE
+    r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7')
     plan_and_execute(
         sciurus17,
         r_arm_waist,
@@ -191,9 +196,10 @@ def main(args=None):
 
     # ハンドを持ち上げる
     r_arm_waist.set_start_state_to_current_state()
-    r_arm_waist.set_goal_state(pose_stamped_msg=PoseStamped(header=Header(frame_id='base_link'),
-                                                            pose=PRE_AND_POST_RELEASE_POSE),
-                               pose_link='r_link7')
+    goal_pose = PoseStamped()
+    goal_pose.header.frame_id='base_link'
+    goal_pose.pose = PRE_AND_POST_RELEASE_POSE
+    r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7')
     plan_and_execute(
         sciurus17,
         r_arm_waist,