頭部、胸部カメラサンプル追加

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
+    ])