From 22ba7f8113d217e43853ca0a79ef5599b8d9877d Mon Sep 17 00:00:00 2001 From: Kuwamai Date: Wed, 21 Feb 2024 11:22:12 +0900 Subject: [PATCH] =?UTF-8?q?=E9=A0=AD=E9=83=A8=E3=80=81=E8=83=B8=E9=83=A8?= =?UTF-8?q?=E3=82=AB=E3=83=A1=E3=83=A9=E3=82=B5=E3=83=B3=E3=83=97=E3=83=AB?= =?UTF-8?q?=E8=BF=BD=E5=8A=A0=20(#153)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * 依存パッケージの追加 * 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 | 53 ++++++ sciurus17_examples/README.md | 38 ++++ .../sciurus17_examples/color_detection.hpp | 42 +++++ .../sciurus17_examples/neck_jt_control.hpp | 39 +++++ .../sciurus17_examples/object_tracker.hpp | 51 ++++++ .../sciurus17_examples/waist_jt_control.hpp | 40 +++++ .../launch/chest_camera_tracking.launch.py | 70 ++++++++ .../launch/head_camera_tracking.launch.py | 70 ++++++++ sciurus17_examples/package.xml | 4 + sciurus17_examples/src/color_detection.cpp | 148 ++++++++++++++++ sciurus17_examples/src/neck_jt_control.cpp | 78 +++++++++ sciurus17_examples/src/object_tracker.cpp | 164 ++++++++++++++++++ sciurus17_examples/src/waist_jt_control.cpp | 73 ++++++++ .../launch/chest_camera.launch.py | 4 +- 14 files changed, 873 insertions(+), 1 deletion(-) create mode 100644 sciurus17_examples/include/sciurus17_examples/color_detection.hpp create mode 100644 sciurus17_examples/include/sciurus17_examples/neck_jt_control.hpp create mode 100644 sciurus17_examples/include/sciurus17_examples/object_tracker.hpp create mode 100644 sciurus17_examples/include/sciurus17_examples/waist_jt_control.hpp create mode 100644 sciurus17_examples/launch/chest_camera_tracking.launch.py create mode 100644 sciurus17_examples/launch/head_camera_tracking.launch.py create mode 100644 sciurus17_examples/src/color_detection.cpp create mode 100644 sciurus17_examples/src/neck_jt_control.cpp create mode 100644 sciurus17_examples/src/object_tracker.cpp create mode 100644 sciurus17_examples/src/waist_jt_control.cpp diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index 728ca6c..1da0016 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/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 diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index 46c8136..58eb169 100644 --- a/sciurus17_examples/README.md +++ b/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) + +--- diff --git a/sciurus17_examples/include/sciurus17_examples/color_detection.hpp b/sciurus17_examples/include/sciurus17_examples/color_detection.hpp new file mode 100644 index 0000000..8d4dfc6 --- /dev/null +++ b/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::SharedPtr image_subscription_; + rclcpp::Publisher::SharedPtr image_annotated_publisher_; + rclcpp::Publisher::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_ diff --git a/sciurus17_examples/include/sciurus17_examples/neck_jt_control.hpp b/sciurus17_examples/include/sciurus17_examples/neck_jt_control.hpp new file mode 100644 index 0000000..249362d --- /dev/null +++ b/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::SharedPtr angles_subscription_; + rclcpp::Publisher::SharedPtr jt_publisher_; + + void angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg); +}; + +} // namespace sciurus17_examples + +#endif // SCIURUS17_EXAMPLES__NECK_JT_CONTROL_HPP_ diff --git a/sciurus17_examples/include/sciurus17_examples/object_tracker.hpp b/sciurus17_examples/include/sciurus17_examples/object_tracker.hpp new file mode 100644 index 0000000..c3057ba --- /dev/null +++ b/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 + +#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::SharedPtr + state_subscription_; + rclcpp::Subscription::SharedPtr object_point_subscription_; + rclcpp::Publisher::SharedPtr angles_publisher_; + control_msgs::msg::JointTrajectoryControllerState::SharedPtr current_angles_msg_; + geometry_msgs::msg::PointStamped::SharedPtr object_point_msg_; + std::vector 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_ diff --git a/sciurus17_examples/include/sciurus17_examples/waist_jt_control.hpp b/sciurus17_examples/include/sciurus17_examples/waist_jt_control.hpp new file mode 100644 index 0000000..4a19957 --- /dev/null +++ b/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::SharedPtr angles_subscription_; + rclcpp::Publisher::SharedPtr jt_publisher_; + + void angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg); +}; + +} // namespace sciurus17_examples + +#endif // SCIURUS17_EXAMPLES__WAIST_JT_CONTROL_HPP_ diff --git a/sciurus17_examples/launch/chest_camera_tracking.launch.py b/sciurus17_examples/launch/chest_camera_tracking.launch.py new file mode 100644 index 0000000..03d9656 --- /dev/null +++ b/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 + ]) diff --git a/sciurus17_examples/launch/head_camera_tracking.launch.py b/sciurus17_examples/launch/head_camera_tracking.launch.py new file mode 100644 index 0000000..4ca5c6b --- /dev/null +++ b/sciurus17_examples/launch/head_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='head_camera_tracking', + remappings=[ + ('/image_raw', '/head_camera/color/image_raw') + ], + extra_arguments=[{'use_intra_process_comms': True}] + ), + ComposableNode( + package='sciurus17_examples', + plugin='sciurus17_examples::ObjectTracker', + name='object_tracker', + namespace='head_camera_tracking', + remappings=[ + ('/controller_state', '/neck_controller/controller_state') + ], + extra_arguments=[{'use_intra_process_comms': True}] + ), + ComposableNode( + package='sciurus17_examples', + plugin='sciurus17_examples::NeckJtControl', + name='neck_jt_control', + namespace='head_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 + ]) diff --git a/sciurus17_examples/package.xml b/sciurus17_examples/package.xml index deefbe9..50c7f53 100644 --- a/sciurus17_examples/package.xml +++ b/sciurus17_examples/package.xml @@ -19,9 +19,13 @@ sciurus17_control sciurus17_description sciurus17_moveit_config + cv_bridge geometry_msgs + image_geometry + libopencv-dev moveit_ros_planning_interface rclcpp + rclcpp_components tf2_geometry_msgs ament_lint_auto diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp new file mode 100644 index 0000000..cf93027 --- /dev/null +++ b/sciurus17_examples/src/color_detection.cpp @@ -0,0 +1,148 @@ +// 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://www.opencv-srf.com/2010/09/object-detection-using-color-seperation.html +// https://docs.opencv.org/4.5.4/d0/d49/tutorial_moments.html + +#include "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" +#include "cv_bridge/cv_bridge.h" +using std::placeholders::_1; + +namespace sciurus17_examples +{ + +ColorDetection::ColorDetection(const rclcpp::NodeOptions & options) +: Node("color_detection", options) +{ + image_subscription_ = this->create_subscription( + "/image_raw", 10, std::bind(&ColorDetection::image_callback, this, _1)); + + image_annotated_publisher_ = + this->create_publisher("image_annotated", 10); + + object_point_publisher_ = + this->create_publisher("target_position", 10); +} + +void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) +{ + // オレンジ色の物体を検出するようにHSVの範囲を設定 + const int LOW_H = 5, HIGH_H = 20; + const int LOW_S = 120, HIGH_S = 255; + const int LOW_V = 120, HIGH_V = 255; + + // 画像全体の10%以上の大きさで映った物体を検出 + const auto MIN_OBJECT_SIZE = msg->width * msg->height * 0.01; + + auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); + + // 画像をRGBからHSVに変換 + cv::Mat img_hsv; + cv::cvtColor(cv_img->image, img_hsv, cv::COLOR_RGB2HSV); + + // 画像の二値化 + cv::Mat img_thresholded; + cv::inRange( + img_hsv, + cv::Scalar(LOW_H, LOW_S, LOW_V), + cv::Scalar(HIGH_H, HIGH_S, HIGH_V), + img_thresholded); + + // ノイズ除去の処理 + cv::morphologyEx( + img_thresholded, + img_thresholded, + cv::MORPH_OPEN, + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); + + // 穴埋めの処理 + cv::morphologyEx( + img_thresholded, + img_thresholded, + cv::MORPH_CLOSE, + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); + + // 検出領域のみを描画 + cv::Mat img_annotated; + cv_img->image.copyTo(img_annotated, img_thresholded); + + // 二値化した領域の輪郭を取得 + std::vector> contours; + cv::findContours(img_thresholded, contours, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE); + + if (contours.size()) { + // 最も面積の大きい領域を取得 + std::vector object_moments; + int max_area_i = -1; + int i = 0; + for (const auto & contour : contours) { + object_moments.push_back(cv::moments(contour)); + if (object_moments[max_area_i].m00 < object_moments[i].m00) { + max_area_i = i; + } + i++; + } + + if (object_moments[max_area_i].m00 > MIN_OBJECT_SIZE) { + // 画像座標系における物体検出位置（2D） + cv::Point2d object_point; + object_point.x = object_moments[max_area_i].m10 / object_moments[max_area_i].m00; + object_point.y = object_moments[max_area_i].m01 / object_moments[max_area_i].m00; + + RCLCPP_DEBUG_STREAM(this->get_logger(), "Detect at" << object_point << "."); + + // 検出領域と検出位置を描画 + const cv::Scalar ANNOTATE_COLOR(256, 0, 256); + const int ANNOTATE_THICKNESS = 4; + const int ANNOTATE_RADIUS = 10; + cv::drawContours(img_annotated, contours, max_area_i, ANNOTATE_COLOR, ANNOTATE_THICKNESS); + cv::circle(img_annotated, object_point, ANNOTATE_RADIUS, ANNOTATE_COLOR, -1); + + // 画像の中心を原点とした検出位置に変換 + cv::Point2d translated_object_point; + translated_object_point.x = object_point.x - msg->width / 2.0; + translated_object_point.y = object_point.y - msg->height / 2.0; + + // 検出位置を-1.0 ~ 1.0に正規化 + cv::Point2d normalized_object_point_; + if (msg->width != 0 && msg->height != 0) { + normalized_object_point_.x = translated_object_point.x / (msg->width / 2.0); + normalized_object_point_.y = translated_object_point.y / (msg->height / 2.0); + } + + // 検出位置を配信 + geometry_msgs::msg::PointStamped object_point_msg; + object_point_msg.header = msg->header; + object_point_msg.point.x = normalized_object_point_.x; + object_point_msg.point.y = normalized_object_point_.y; + object_point_publisher_->publish(object_point_msg); + } + } + + // 閾値による二値化画像を配信 + sensor_msgs::msg::Image::SharedPtr img_annotated_msg = + cv_bridge::CvImage(msg->header, msg->encoding, img_annotated).toImageMsg(); + image_annotated_publisher_->publish(*img_annotated_msg); +} + +} // namespace sciurus17_examples + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::ColorDetection) diff --git a/sciurus17_examples/src/neck_jt_control.cpp b/sciurus17_examples/src/neck_jt_control.cpp new file mode 100644 index 0000000..76fbe30 --- /dev/null +++ b/sciurus17_examples/src/neck_jt_control.cpp @@ -0,0 +1,78 @@ +// 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. + +#include "sciurus17_examples/neck_jt_control.hpp" + +#include "angles/angles.h" + +#include "rclcpp/rclcpp.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" + +using std::placeholders::_1; +using namespace std::chrono_literals; + +namespace sciurus17_examples +{ + +NeckJtControl::NeckJtControl(const rclcpp::NodeOptions & options) +: Node("neck_control", options) +{ + angles_subscription_ = this->create_subscription( + "target_angles", 10, std::bind(&NeckJtControl::angles_callback, this, _1)); + + jt_publisher_ = this->create_publisher( + "/neck_controller/joint_trajectory", 10); +} + +void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) +{ + // 動作時間 + const auto TIME_FROM_START = 1ms; + // 首可動範囲 + const double MAX_YAW_ANGLE = angles::from_degrees(120); + const double MIN_YAW_ANGLE = angles::from_degrees(-120); + const double MAX_PITCH_ANGLE = angles::from_degrees(50); + const double MIN_PITCH_ANGLE = angles::from_degrees(-75); + + // 角度指令値取得 + if (msg->data.size() != 2) { + return; + } + auto yaw_angle = msg->data[0]; + auto pitch_angle = msg->data[1]; + + // 角度指令値を可動範囲内にする + yaw_angle = std::clamp(yaw_angle, MIN_YAW_ANGLE, MAX_YAW_ANGLE); + pitch_angle = std::clamp(pitch_angle, MIN_PITCH_ANGLE, MAX_PITCH_ANGLE); + + // joint名設定 + trajectory_msgs::msg::JointTrajectory jt_msg; + jt_msg.joint_names.push_back("neck_yaw_joint"); + jt_msg.joint_names.push_back("neck_pitch_joint"); + + // 角度指令値設定 + trajectory_msgs::msg::JointTrajectoryPoint jt_point_msg; + jt_point_msg.positions.push_back(yaw_angle); + jt_point_msg.positions.push_back(pitch_angle); + jt_point_msg.time_from_start = rclcpp::Duration(TIME_FROM_START); + jt_msg.points.push_back(jt_point_msg); + + jt_publisher_->publish(jt_msg); +} + +} // namespace sciurus17_examples + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::NeckJtControl) diff --git a/sciurus17_examples/src/object_tracker.cpp b/sciurus17_examples/src/object_tracker.cpp new file mode 100644 index 0000000..b994d59 --- /dev/null +++ b/sciurus17_examples/src/object_tracker.cpp @@ -0,0 +1,164 @@ +// 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. + +#include "sciurus17_examples/object_tracker.hpp" + +#include "angles/angles.h" + +#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" +using std::placeholders::_1; +using namespace std::chrono_literals; + +namespace sciurus17_examples +{ + +ObjectTracker::ObjectTracker(const rclcpp::NodeOptions & options) +: Node("object_tracker", options) +{ + timer_ = this->create_wall_timer( + 30ms, std::bind(&ObjectTracker::tracking, this)); + + state_subscription_ = + this->create_subscription( + "/controller_state", 10, std::bind(&ObjectTracker::state_callback, this, _1)); + + object_point_subscription_ = this->create_subscription( + "target_position", 10, std::bind(&ObjectTracker::point_callback, this, _1)); + + angles_publisher_ = + this->create_publisher("target_angles", 10); +} + +void ObjectTracker::state_callback( + const control_msgs::msg::JointTrajectoryControllerState::SharedPtr msg) +{ + current_angles_msg_ = msg; +} + +void ObjectTracker::point_callback(const geometry_msgs::msg::PointStamped::SharedPtr msg) +{ + object_point_msg_ = msg; +} + +void ObjectTracker::tracking() +{ + // 追従を開始する物体位置の閾値 + const double POSITION_THRESH = 0.1; + + // 角度指令値の初期値 + const std::vector INITIAL_ANGLES = {0, 0}; + + // 可動範囲 + const double MAX_YAW_ANGLE = angles::from_degrees(120); + const double MIN_YAW_ANGLE = angles::from_degrees(-120); + const double MAX_PITCH_ANGLE = angles::from_degrees(50); + const double MIN_PITCH_ANGLE = angles::from_degrees(-75); + + // 腰、首の関節数 + const int WAIST_JOINT_NUM = 1; + const int NECK_JOINT_NUM = 2; + + // 最大角度変化量 + const double MAX_ANGULAR_DIFF = angles::from_degrees(1.5); + + // 初期角度へ戻る際の角度変化量 + const double RESET_ANGULAR_DIFF = angles::from_degrees(0.5); + + // 物体が検出されなくなってから初期角度に戻り始めるまでの時間 + const std::chrono::nanoseconds DETECTION_TIMEOUT = 1s; + + // 追従速度ゲイン + // 値が大きいほど追従速度が速くなる + const double OPERATION_GAIN = 0.02; + + // 追従フラグ + bool look_object = false; + + // 現在の関節角度を取得 + if (!current_angles_msg_) { + RCLCPP_INFO_STREAM(this->get_logger(), "Waiting controller state."); + return; + } + if (current_angles_msg_->feedback.positions.size() != WAIST_JOINT_NUM && + current_angles_msg_->feedback.positions.size() != NECK_JOINT_NUM) + { + return; + } + const auto current_angles = current_angles_msg_->feedback.positions; + if (target_angles_.empty()) { + target_angles_ = current_angles; + if (target_angles_.size() == 1) { + target_angles_.push_back(0); + } + } + + // 現在時刻取得 + auto now = this->get_clock()->now().nanoseconds(); + + if (object_point_msg_) { + // タイムアウト時間以内に物体を検出したとき追従フラグをtrueにする + const auto detected_time = rclcpp::Time(object_point_msg_->header.stamp).nanoseconds(); + const auto POINT_ELAPSED_TIME = now - detected_time; + look_object = POINT_ELAPSED_TIME < DETECTION_TIMEOUT.count(); + } + + // 物体が検出されたら追従を行う + if (look_object) { + // 物体検出位置を取得 + std::vector object_position; + object_position.push_back(object_point_msg_->point.x); + object_position.push_back(object_point_msg_->point.y); + std::vector diff_angles = {0, 0}; + + // 追従動作のための角度を計算 + for (int i = 0; i < 2; i++) { + if (std::abs(object_position[i]) > POSITION_THRESH) { + diff_angles[i] = object_position[i] * OPERATION_GAIN; + diff_angles[i] = std::clamp(diff_angles[i], -MAX_ANGULAR_DIFF, MAX_ANGULAR_DIFF); + target_angles_[i] -= diff_angles[i]; + } + } + } else { + // ゆっくりと初期角度へ戻る + std::vector diff_angles = {0, 0}; + + for (int i = 0; i < 2; i++) { + diff_angles[i] = INITIAL_ANGLES[i] - target_angles_[i]; + if (std::abs(diff_angles[i]) > RESET_ANGULAR_DIFF) { + target_angles_[i] += std::copysign(RESET_ANGULAR_DIFF, diff_angles[i]); + } else { + target_angles_[i] = INITIAL_ANGLES[i]; + } + } + } + + // 角度指令値を可動範囲内にする + target_angles_[0] = std::clamp(target_angles_[0], MIN_YAW_ANGLE, MAX_YAW_ANGLE); + target_angles_[1] = std::clamp(target_angles_[1], MIN_PITCH_ANGLE, MAX_PITCH_ANGLE); + + // 角度指令値を配信する + std_msgs::msg::Float64MultiArray target_angles_msg; + target_angles_msg.data.push_back(target_angles_[0]); + target_angles_msg.data.push_back(target_angles_[1]); + angles_publisher_->publish(target_angles_msg); +} + +} // namespace sciurus17_examples + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::ObjectTracker) diff --git a/sciurus17_examples/src/waist_jt_control.cpp b/sciurus17_examples/src/waist_jt_control.cpp new file mode 100644 index 0000000..f99aaf2 --- /dev/null +++ b/sciurus17_examples/src/waist_jt_control.cpp @@ -0,0 +1,73 @@ +// 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. + +#include "sciurus17_examples/waist_jt_control.hpp" + +#include "angles/angles.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "control_msgs/action/follow_joint_trajectory.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" + +using std::placeholders::_1; +using namespace std::chrono_literals; + +namespace sciurus17_examples +{ + +WaistJtControl::WaistJtControl(const rclcpp::NodeOptions & options) +: Node("waist_control", options) +{ + angles_subscription_ = this->create_subscription( + "target_angles", 10, std::bind(&WaistJtControl::angles_callback, this, _1)); + + jt_publisher_ = this->create_publisher( + "/waist_yaw_controller/joint_trajectory", 10); +} + +void WaistJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) +{ + // 動作時間 + const auto TIME_FROM_START = 1ms; + // 首可動範囲 + const double MAX_YAW_ANGLE = angles::from_degrees(120); + const double MIN_YAW_ANGLE = angles::from_degrees(-120); + + // 角度指令値取得 + if (msg->data.size() != 2) { + return; + } + auto yaw_angle = msg->data[0]; + + // 角度指令値を可動範囲内にする + yaw_angle = std::clamp(yaw_angle, MIN_YAW_ANGLE, MAX_YAW_ANGLE); + + // joint名設定 + trajectory_msgs::msg::JointTrajectory jt_msg; + jt_msg.joint_names.push_back("waist_yaw_joint"); + + // 角度指令値設定 + trajectory_msgs::msg::JointTrajectoryPoint jt_point_msg; + jt_point_msg.positions.push_back(yaw_angle); + jt_point_msg.time_from_start = rclcpp::Duration(TIME_FROM_START); + jt_msg.points.push_back(jt_point_msg); + + jt_publisher_->publish(jt_msg); +} + +} // namespace sciurus17_examples + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::WaistJtControl) diff --git a/sciurus17_vision/launch/chest_camera.launch.py b/sciurus17_vision/launch/chest_camera.launch.py index e8599a7..8c2bb1e 100644 --- a/sciurus17_vision/launch/chest_camera.launch.py +++ b/sciurus17_vision/launch/chest_camera.launch.py @@ -12,6 +12,8 @@ # 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 launch import LaunchDescription from launch_ros.actions import Node @@ -25,7 +27,7 @@ def generate_launch_description(): executable='usb_cam_node_exe', namespace='chest_camera', parameters=[ - {'video_device': '/dev/chestcamera'}, + {'video_device': os.path.realpath('/dev/chestcamera')}, {'frame_id': 'chest_camera_link'}, {'image_width': 1280}, {'image_height': 720},