PythonとC++のサンプルコードに対するリファクタリングの実施

sciurus17_examples/include/pose_presets.hpp

@@ -15,14 +15,14 @@
 #ifndef POSE_PRESETS_HPP_
 #define POSE_PRESETS_HPP_
 
-#include "geometry_msgs/msg/pose.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);
+  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);
 // 左グリッパを下に向ける姿勢を作成

sciurus17_examples/include/sciurus17_examples/color_detection_2d.hpp

@@ -15,25 +15,33 @@
 #ifndef SCIURUS17_EXAMPLES__COLOR_DETECTION_2D_HPP_
 #define SCIURUS17_EXAMPLES__COLOR_DETECTION_2D_HPP_
 
-#include "rclcpp/rclcpp.hpp"
-#include "geometry_msgs/msg/point_stamped.hpp"
-#include "sensor_msgs/msg/image.hpp"
-#include "opencv2/opencv.hpp"
+#include <geometry_msgs/msg/point_stamped.hpp>
+#include <opencv2/opencv.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
 
 namespace sciurus17_examples
 {
 
+// 画像からオレンジ色の物体を検出し、その位置を配信するコンポーネントノード
+// ROS 2のコンポーネントとして登録され、別プロセスから動的にロード可能
 class ColorDetection2D : public rclcpp::Node
 {
 public:
   explicit ColorDetection2D(const rclcpp::NodeOptions & options);
 
 private:
+  // 画像トピック（/image_raw）を購読するサブスクライバ
   rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_subscription_;
+
+  // 検出結果を描画した画像を配信するパブリッシャ
   rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_annotated_publisher_;
+
+  // 検出した物体の正規化座標（-1.0～1.0）を配信するパブリッシャ
   rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr object_point_publisher_;
-  rclcpp::TimerBase::SharedPtr timer_;
 
+  // 画像トピックを受信したときに呼ばれるコールバック関数
+  // HSV色空間で物体を検出し、画像中心を原点とした正規化座標を計算する
   void image_callback(const sensor_msgs::msg::Image::SharedPtr msg);
 };
 

sciurus17_examples/include/sciurus17_examples/neck_jt_control.hpp

@@ -15,22 +15,29 @@
 #ifndef SCIURUS17_EXAMPLES__NECK_JT_CONTROL_HPP_
 #define SCIURUS17_EXAMPLES__NECK_JT_CONTROL_HPP_
 
-#include "rclcpp/rclcpp.hpp"
-#include "trajectory_msgs/msg/joint_trajectory.hpp"
-#include "std_msgs/msg/float64_multi_array.hpp"
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
+#include <trajectory_msgs/msg/joint_trajectory.hpp>
 
 namespace sciurus17_examples
 {
 
+// 首の2軸（yaw, pitch）を制御するコンポーネントノード
+// 目標角度をFloat64MultiArrayで受け取り、JointTrajectoryメッセージに変換して配信
 class NeckJtControl : public rclcpp::Node
 {
 public:
   explicit NeckJtControl(const rclcpp::NodeOptions & options);
 
 private:
+  // 目標角度[yaw, pitch]を購読するサブスクライバ
   rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr angles_subscription_;
+
+  // JointTrajectoryメッセージを配信するパブリッシャ（ros2_controllersが購読）
   rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr jt_publisher_;
 
+  // 目標角度を受信したときに呼ばれるコールバック関数
+  // 可動範囲チェックとJointTrajectoryメッセージへの変換を行う
   void angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
 };
 

sciurus17_examples/include/sciurus17_examples/object_tracker.hpp

@@ -17,33 +17,60 @@
 
 #include <vector>
 
-#include "rclcpp/rclcpp.hpp"
-#include "control_msgs/msg/joint_trajectory_controller_state.hpp"
-#include "geometry_msgs/msg/point_stamped.hpp"
-#include "sensor_msgs/msg/image.hpp"
-#include "std_msgs/msg/float64_multi_array.hpp"
+#include <control_msgs/msg/joint_trajectory_controller_state.hpp>
+#include <geometry_msgs/msg/point_stamped.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
 
 namespace sciurus17_examples
 {
 
+// 検出した物体を追従するコンポーネントノード
+// ColorDetection2Dノードから物体位置を受け取り、首/腰の目標角度を計算して配信
+// 30msタイマーで追従制御ループを実行
 class ObjectTracker : public rclcpp::Node
 {
 public:
   explicit ObjectTracker(const rclcpp::NodeOptions & options);
 
 private:
+  // 30msごとに追従制御を実行するタイマー
   rclcpp::TimerBase::SharedPtr timer_;
+
+  // 首/腰コントローラの現在角度を購読するサブスクライバ
   rclcpp::Subscription<control_msgs::msg::JointTrajectoryControllerState>::SharedPtr
     state_subscription_;
+
+  // 物体の正規化座標を購読するサブスクライバ（ColorDetection2Dが配信）
   rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr object_point_subscription_;
+
+  // 首/腰の目標角度を配信するパブリッシャ（NeckJtControl, WaistJtControlが購読）
   rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr angles_publisher_;
+
+  // 最新の関節角度を保持
   control_msgs::msg::JointTrajectoryControllerState::SharedPtr current_angles_msg_;
+
+  // 最新の物体検出位置を保持
   geometry_msgs::msg::PointStamped::SharedPtr object_point_msg_;
+
+  // 現在の目標角度[yaw, pitch]を保持
   std::vector<double> target_angles_;
 
+  // コントローラ状態を受信したときに呼ばれるコールバック関数
   void state_callback(const control_msgs::msg::JointTrajectoryControllerState::SharedPtr msg);
+
+  // 物体位置を受信したときに呼ばれるコールバック関数
   void point_callback(const geometry_msgs::msg::PointStamped::SharedPtr msg);
+
+  // タイマーごとに呼ばれる追従制御ループ
+  // 物体位置に基づいて目標角度を更新し配信
   void tracking();
+
+  // 物体の正規化座標から目標角度（追従方向）を更新する
+  void update_target_angles_for_tracking(const std::vector<double> & object_position);
+
+  // 目標角度を初期姿勢へゆっくり近づける
+  void update_target_angles_for_reset();
 };
 
 }  // namespace sciurus17_examples

sciurus17_examples/include/sciurus17_examples/waist_jt_control.hpp

@@ -15,23 +15,29 @@
 #ifndef SCIURUS17_EXAMPLES__WAIST_JT_CONTROL_HPP_
 #define SCIURUS17_EXAMPLES__WAIST_JT_CONTROL_HPP_
 
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_action/rclcpp_action.hpp"
-#include "trajectory_msgs/msg/joint_trajectory.hpp"
-#include "std_msgs/msg/float64_multi_array.hpp"
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
+#include <trajectory_msgs/msg/joint_trajectory.hpp>
 
 namespace sciurus17_examples
 {
 
+// 腰の1軸（yaw）を制御するコンポーネントノード
+// 目標角度をFloat64MultiArrayで受け取り、JointTrajectoryメッセージに変換して配信
 class WaistJtControl : public rclcpp::Node
 {
 public:
   explicit WaistJtControl(const rclcpp::NodeOptions & options);
 
 private:
+  // 目標角度[yaw]を購読するサブスクライバ
   rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr angles_subscription_;
+
+  // JointTrajectoryメッセージを配信するパブリッシャ（ros2_controllersが購読）
   rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr jt_publisher_;
 
+  // 目標角度を受信したときに呼ばれるコールバック関数
+  // 可動範囲チェックとJointTrajectoryメッセージへの変換を行う
   void angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
 };
 

sciurus17_examples/launch/camera_example.launch.py

@@ -18,22 +18,23 @@
 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
+from sciurus17_description.robot_description_loader import (
+    RobotDescriptionLoader,
+)
 
 
 def generate_launch_description():
     declare_example_name = DeclareLaunchArgument(
         'example',
         default_value='point_cloud_detection',
-        description=(
-            'Set an example executable name: '
-            '[aruco_detection, color_detection, point_cloud_detection]'
-        ),
+        choices=['aruco_detection', 'color_detection', 'point_cloud_detection'],
+        description='Set an example executable name.',
     )
 
     declare_use_sim_time = DeclareLaunchArgument(
         'use_sim_time',
         default_value='false',
+        choices=['true', 'false'],
         description=('Set true when using the gazebo simulator.'),
     )
 
@@ -63,7 +64,9 @@ def generate_launch_description():
         [
             declare_example_name,
             declare_use_sim_time,
-            SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')),
+            SetParameter(
+                name='use_sim_time', value=LaunchConfiguration('use_sim_time')
+            ),
             picking_node,
             detection_node,
         ]

sciurus17_examples/launch/chest_camera_tracking.launch.py

@@ -24,12 +24,13 @@ def generate_launch_description():
     declare_use_sim_time = DeclareLaunchArgument(
         'use_sim_time',
         default_value='false',
+        choices=['true', 'false'],
         description=('Set true when using the gazebo simulator.'),
     )
 
     container = ComposableNodeContainer(
         name='tracking_container',
-        namespace='head_camera_tracking',
+        namespace='chest_camera_tracking',
         package='rclcpp_components',
         executable='component_container',
         output='screen',
@@ -47,7 +48,12 @@ def generate_launch_description():
                 namespace='chest_camera_tracking',
                 package='sciurus17_examples',
                 plugin='sciurus17_examples::ObjectTracker',
-                remappings=[('/controller_state', '/waist_yaw_controller/controller_state')],
+                remappings=[
+                    (
+                        '/controller_state',
+                        '/waist_yaw_controller/controller_state',
+                    )
+                ],
                 extra_arguments=[{'use_intra_process_comms': True}],
             ),
             ComposableNode(
@@ -63,7 +69,9 @@ def generate_launch_description():
     return LaunchDescription(
         [
             declare_use_sim_time,
-            SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')),
+            SetParameter(
+                name='use_sim_time', value=LaunchConfiguration('use_sim_time')
+            ),
             container,
         ]
     )

sciurus17_examples/launch/demo.launch.py

@@ -24,16 +24,23 @@
 
 def generate_launch_description():
     declare_use_head_camera = DeclareLaunchArgument(
-        'use_head_camera', default_value='true', description='Use head camera.'
+        'use_head_camera',
+        default_value='true',
+        choices=['true', 'false'],
+        description='Use head camera.',
     )
 
     declare_use_chest_camera = DeclareLaunchArgument(
-        'use_chest_camera', default_value='true', description='Use chest camera.'
+        'use_chest_camera',
+        default_value='true',
+        choices=['true', 'false'],
+        description='Use chest camera.',
     )
 
     declare_use_sim_time = DeclareLaunchArgument(
         'use_sim_time',
         default_value='false',
+        choices=['true', 'false'],
         description=('Set true when using the simulator.'),
     )
 
@@ -57,22 +64,30 @@ def generate_launch_description():
 
     head_camera_node = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            [get_package_share_directory('sciurus17_vision'), '/launch/head_camera.launch.py']
+            [
+                get_package_share_directory('sciurus17_vision'),
+                '/launch/head_camera.launch.py',
+            ]
         ),
         condition=IfCondition(LaunchConfiguration('use_head_camera')),
     )
 
     chest_camera_node = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            [get_package_share_directory('sciurus17_vision'), '/launch/chest_camera.launch.py']
+            [
+                get_package_share_directory('sciurus17_vision'),
+                '/launch/chest_camera.launch.py',
+            ]
         ),
         condition=IfCondition(LaunchConfiguration('use_chest_camera')),
     )
 
     return LaunchDescription(
         [
             declare_use_sim_time,
-            SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')),
+            SetParameter(
+                name='use_sim_time', value=LaunchConfiguration('use_sim_time')
+            ),
             declare_use_head_camera,
             declare_use_chest_camera,
             move_group,

sciurus17_examples/launch/example.launch.py

@@ -18,23 +18,31 @@
 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
+from sciurus17_description.robot_description_loader import (
+    RobotDescriptionLoader,
+)
 
 
 def generate_launch_description():
     declare_example_name = DeclareLaunchArgument(
         'example',
         default_value='gripper_control',
-        description=(
-            'Set an example executable name: '
-            '[gripper_control, pose_groupstate, joint_values, neck_control, waist_control,'
-            'pick_and_place_right_arm_waist, pick_and_place_left_arm, head_camera_tracking, '
-            'chest_camera_tracking]'
-        ),
+        choices=[
+            'gripper_control',
+            'pose_groupstate',
+            'joint_values',
+            'cartesian_path',
+            'neck_control',
+            'waist_control',
+            'pick_and_place_right_arm_waist',
+            'pick_and_place_left_arm',
+        ],
+        description='Set an example executable name.',
     )
     declare_use_sim_time = DeclareLaunchArgument(
         'use_sim_time',
         default_value='false',
+        choices=['true', 'false'],
         description=('Set true when using the gazebo simulator.'),
     )
 

sciurus17_examples/launch/head_camera_tracking.launch.py

@@ -24,6 +24,7 @@ def generate_launch_description():
     declare_use_sim_time = DeclareLaunchArgument(
         'use_sim_time',
         default_value='false',
+        choices=['true', 'false'],
         description=('Set true when using the gazebo simulator.'),
     )
 
@@ -47,7 +48,9 @@ def generate_launch_description():
                 namespace='head_camera_tracking',
                 package='sciurus17_examples',
                 plugin='sciurus17_examples::ObjectTracker',
-                remappings=[('/controller_state', '/neck_controller/controller_state')],
+                remappings=[
+                    ('/controller_state', '/neck_controller/controller_state')
+                ],
                 extra_arguments=[{'use_intra_process_comms': True}],
             ),
             ComposableNode(
@@ -63,7 +66,9 @@ def generate_launch_description():
     return LaunchDescription(
         [
             declare_use_sim_time,
-            SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')),
+            SetParameter(
+                name='use_sim_time', value=LaunchConfiguration('use_sim_time')
+            ),
             container,
         ]
     )