頭部、胸部カメラサンプル追加 (#153)

* 依存パッケージの追加

* head_camera_tracking MoveIt版

* クラスの分割

* トラッキング動作をmain関数に移動

* 依存パッケージ追加

* 不要な行の削除

* コメント追加

* 制御周期変更

* 追従処理をクラス化

* 不要な変数を削除

* component node化

* install記述追加

* rename

* 不要な変数を削除

* 配列サイズのチェック

* yaw、pitch軸の処理をfor文に置き換え

* action serverの確認をコンストラクタ内で実行

* プロセス内通信を有効化

* action動作が完了したか確認する処理を追加

* コメント修正

* コメント修正

* 角度制限の定義箇所を変更

* コメント修正

* スタイル修正

* パラメータ調整

* スタイル修正

* use_sim_timeオプション追加

* コメント修正

* README更新

* 不要な記述を削除

* 依存パッケージ修正

* ディレクトリ名変更

* 誤字修正

* joint_trajectoryの配信をactionからtopicに変更

* 動作周期調整

* 腰追従動作用コード追加

* ファイル名変更

* waist_jt_controller追加

* Topic名変更

* chest_camera_tracking.launch.py追加

* namespace変更

* スタイル修正

* Update README

* 胸部カメラパス変更

* 不要な処理を削除

* 不要なパッケージを削除

* 不要なコメントの削除

* 腰、首の関節数を変数化

* RCLCPP_INFOをDEBUGに変更

* topic名を抽象的なものに変更

* 一番大きく映った領域のみを検出

* remap修正

* 誤字修正

* パラメータ調整

* 変数名変更

sciurus17_examples/CMakeLists.txt

@@ -18,10 +18,57 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
+find_package(cv_bridge REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(control_msgs REQUIRED)
+find_package(image_geometry REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
+find_package(OpenCV REQUIRED COMPONENTS core)
 find_package(rclcpp REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_components REQUIRED)
+
+include_directories(include)
+
+set(components_list
+  color_detection
+  neck_jt_control
+  object_tracker
+  waist_jt_control
+)
+foreach(loop_var IN LISTS components_list)
+  add_library(${loop_var} SHARED
+    src/${loop_var}.cpp)
+  target_compile_definitions(${loop_var}
+    PRIVATE "COMPOSITION_BUILDING_DLL")
+  ament_target_dependencies(${loop_var}
+    angles
+    rclcpp_components
+    rclcpp_action
+    cv_bridge
+    geometry_msgs
+    image_geometry
+    OpenCV
+    rclcpp
+    control_msgs)
+endforeach()
+
+rclcpp_components_register_nodes(color_detection "sciurus17_examples::ColorDetection")
+rclcpp_components_register_nodes(neck_jt_control "sciurus17_examples::NeckJtControl")
+rclcpp_components_register_nodes(object_tracker "sciurus17_examples::ObjectTracker")
+rclcpp_components_register_nodes(waist_jt_control "sciurus17_examples::WaistJtControl")
+
+install(TARGETS
+  color_detection
+  neck_jt_control
+  object_tracker
+  waist_jt_control
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
 
 set(LIBRARY_NAME "pose_presets")
 add_library(${LIBRARY_NAME}
@@ -73,9 +120,15 @@ 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}
+    rclcpp_action
+    cv_bridge
     geometry_msgs
+    image_geometry
     moveit_ros_planning_interface
+    OpenCV
     rclcpp
+    tf2_geometry_msgs
+    control_msgs
   )
 
   install(TARGETS

sciurus17_examples/README.md

@@ -17,6 +17,8 @@
     - [waist\_control](#waist_control)
     - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist)
     - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm)
+    - [head\_camera\_tracking](#head_camera_tracking)
+    - [chest\_camera\_tracking](#chest_camera_tracking)
 
 ## 準備（実機を使う場合）
 
@@ -89,6 +91,8 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' use_
 - [waist\_control](#waist_control)
 - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist)
 - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm)
+- [head\_camera\_tracking](#head_camera_tracking)
+- [chest\_camera\_tracking](#chest_camera_tracking)
 
 実行できるサンプルの一覧は、`example.launch.py`にオプション`-s`を付けて実行することで表示できます。
 
@@ -165,3 +169,37 @@ ros2 launch sciurus17_examples example.launch.py example:='pick_and_place_left_a
 [back to example list](#examples)
 
 ---
+
+### head_camera_tracking
+
+頭部カメラ映像を用いてオレンジ色の物体を追従するコード例です。
+
+Gazeboで実行する場合は動作環境によってうまく追従しない場合があります。
+カメラ解像度やサンプルコード内の追従速度ゲインを調整してください。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch sciurus17_examples head_camera_tracking.launch.py
+```
+
+[back to example list](#examples)
+
+---
+
+### chest_camera_tracking
+
+胸部カメラ映像を用いてオレンジ色の物体を追従するコード例です。
+
+Gazeboで実行する場合は動作環境によってうまく追従しない場合があります。
+カメラ解像度やサンプルコード内の追従速度ゲインを調整してください。
+
+次のコマンドを実行します。
+
+```sh
+ros2 launch sciurus17_examples chest_camera_tracking.launch.py
+```
+
+[back to example list](#examples)
+
+---

sciurus17_examples/include/sciurus17_examples/color_detection.hpp

@@ -0,0 +1,42 @@
+// 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 SCIURUS17_EXAMPLES__COLOR_DETECTION_HPP_
+#define SCIURUS17_EXAMPLES__COLOR_DETECTION_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/point_stamped.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "opencv2/opencv.hpp"
+
+namespace sciurus17_examples
+{
+
+class ColorDetection : public rclcpp::Node
+{
+public:
+  explicit ColorDetection(const rclcpp::NodeOptions & options);
+
+private:
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_subscription_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_annotated_publisher_;
+  rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr object_point_publisher_;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  void image_callback(const sensor_msgs::msg::Image::SharedPtr msg);
+};
+
+}  // namespace sciurus17_examples
+
+#endif  // SCIURUS17_EXAMPLES__COLOR_DETECTION_HPP_

sciurus17_examples/include/sciurus17_examples/neck_jt_control.hpp

@@ -0,0 +1,39 @@
+// 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 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"
+
+namespace sciurus17_examples
+{
+
+class NeckJtControl : public rclcpp::Node
+{
+public:
+  explicit NeckJtControl(const rclcpp::NodeOptions & options);
+
+private:
+  rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr angles_subscription_;
+  rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr jt_publisher_;
+
+  void angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
+};
+
+}  // namespace sciurus17_examples
+
+#endif  // SCIURUS17_EXAMPLES__NECK_JT_CONTROL_HPP_

sciurus17_examples/include/sciurus17_examples/object_tracker.hpp

@@ -0,0 +1,51 @@
+// 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 SCIURUS17_EXAMPLES__OBJECT_TRACKER_HPP_
+#define SCIURUS17_EXAMPLES__OBJECT_TRACKER_HPP_
+
+#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"
+
+namespace sciurus17_examples
+{
+
+class ObjectTracker : public rclcpp::Node
+{
+public:
+  explicit ObjectTracker(const rclcpp::NodeOptions & options);
+
+private:
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Subscription<control_msgs::msg::JointTrajectoryControllerState>::SharedPtr
+    state_subscription_;
+  rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr object_point_subscription_;
+  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_;
+  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();
+};
+
+}  // namespace sciurus17_examples
+
+#endif  // SCIURUS17_EXAMPLES__OBJECT_TRACKER_HPP_

sciurus17_examples/include/sciurus17_examples/waist_jt_control.hpp

@@ -0,0 +1,40 @@
+// 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 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"
+
+namespace sciurus17_examples
+{
+
+class WaistJtControl : public rclcpp::Node
+{
+public:
+  explicit WaistJtControl(const rclcpp::NodeOptions & options);
+
+private:
+  rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr angles_subscription_;
+  rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr jt_publisher_;
+
+  void angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
+};
+
+}  // namespace sciurus17_examples
+
+#endif  // SCIURUS17_EXAMPLES__WAIST_JT_CONTROL_HPP_

sciurus17_examples/launch/chest_camera_tracking.launch.py

@@ -0,0 +1,70 @@
+# 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.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.actions import SetParameter
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+    declare_use_sim_time = DeclareLaunchArgument(
+        'use_sim_time', default_value='false',
+        description=('Set true when using the gazebo simulator.')
+    )
+
+    container = ComposableNodeContainer(
+        name='tracking_container',
+        namespace='head_camera_tracking',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='sciurus17_examples',
+                plugin='sciurus17_examples::ColorDetection',
+                name='color_detection',
+                namespace='chest_camera_tracking',
+                remappings=[
+                    ('/image_raw', '/chest_camera/image_raw')
+                ],
+                extra_arguments=[{'use_intra_process_comms': True}]
+                ),
+            ComposableNode(
+                package='sciurus17_examples',
+                plugin='sciurus17_examples::ObjectTracker',
+                name='object_tracker',
+                namespace='chest_camera_tracking',
+                remappings=[
+                    ('/controller_state', '/waist_yaw_controller/controller_state')
+                ],
+                extra_arguments=[{'use_intra_process_comms': True}]
+                ),
+            ComposableNode(
+                package='sciurus17_examples',
+                plugin='sciurus17_examples::WaistJtControl',
+                name='waist_jt_control',
+                namespace='chest_camera_tracking',
+                extra_arguments=[{'use_intra_process_comms': True}]
+                ),
+        ],
+        output='screen',
+    )
+
+    return LaunchDescription([
+        declare_use_sim_time,
+        SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')),
+        container
+    ])