From 943a2fd27cc19905364b8934815cc5478b93bf22 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Fri, 24 Nov 2023 17:44:21 +0900 Subject: [PATCH 01/55] =?UTF-8?q?=E4=BE=9D=E5=AD=98=E3=83=91=E3=83=83?= =?UTF-8?q?=E3=82=B1=E3=83=BC=E3=82=B8=E3=81=AE=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/CMakeLists.txt | 12 ++++++++++++ sciurus17_examples/package.xml | 5 +++++ 2 files changed, 17 insertions(+) diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index 728ca6c..e696b3e 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/sciurus17_examples/CMakeLists.txt @@ -18,8 +18,13 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(cv_bridge REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(image_geometry REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) +find_package(OpenCV REQUIRED COMPONENTS core) +find_package(pcl_conversions REQUIRED) +find_package(pcl_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -68,14 +73,21 @@ set(executable_list waist_control pick_and_place_right_arm_waist pick_and_place_left_arm + head_camera_tracking ) 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} + cv_bridge geometry_msgs + image_geometry moveit_ros_planning_interface + OpenCV + pcl_ros + pcl_conversions rclcpp + tf2_geometry_msgs ) install(TARGETS diff --git a/sciurus17_examples/package.xml b/sciurus17_examples/package.xml index deefbe9..1c8557e 100644 --- a/sciurus17_examples/package.xml +++ b/sciurus17_examples/package.xml @@ -19,9 +19,14 @@ sciurus17_control sciurus17_description sciurus17_moveit_config + cv_bridge geometry_msgs + image_geometry + libopencv-dev moveit_ros_planning_interface + pcl_ros rclcpp + realsense2_camera tf2_geometry_msgs ament_lint_auto From 60e08ba027dd7e3a00f836d4ff5fdf14f86cce29 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Fri, 24 Nov 2023 17:45:28 +0900 Subject: [PATCH 02/55] =?UTF-8?q?head=5Fcamera=5Ftracking=20MoveIt?= =?UTF-8?q?=E7=89=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/head_camera_tracking.cpp | 150 ++++++++++++++++++ 1 file changed, 150 insertions(+) create mode 100644 sciurus17_examples/src/head_camera_tracking.cpp diff --git a/sciurus17_examples/src/head_camera_tracking.cpp b/sciurus17_examples/src/head_camera_tracking.cpp new file mode 100644 index 0000000..76fe248 --- /dev/null +++ b/sciurus17_examples/src/head_camera_tracking.cpp @@ -0,0 +1,150 @@ +// 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 + +#include +#include +#include +#include + +#include "moveit/move_group_interface/move_group_interface.h" +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2_ros/transform_broadcaster.h" +#include "opencv2/opencv.hpp" +#include "opencv2/imgproc/imgproc.hpp" +#include "cv_bridge/cv_bridge.h" +#include "image_geometry/pinhole_camera_model.h" +using std::placeholders::_1; +using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; + +class HeadCameraTracking : public rclcpp::Node +{ +public: + HeadCameraTracking( + rclcpp::Node::SharedPtr move_group_neck_node) + : Node("head_camera_tracking") + { + move_group_neck_ = std::make_shared(move_group_neck_node, "neck_group"); + move_group_neck_->setMaxVelocityScalingFactor(0.5); + move_group_neck_->setMaxAccelerationScalingFactor(0.5); + + move_group_neck_->setNamedTarget("neck_init_pose"); + move_group_neck_->move(); + + image_subscription_ = this->create_subscription( + "/head_camera/color/image_raw", 10, std::bind(&HeadCameraTracking::image_callback, this, _1)); + + image_thresholded_publisher_ = + this->create_publisher("image_thresholded", 10); + } + +private: + rclcpp::Subscription::SharedPtr image_subscription_; + rclcpp::Publisher::SharedPtr image_thresholded_publisher_; + std::shared_ptr move_group_neck_; + + void 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; + + // ウェブカメラの画像を受け取る + auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); + + // 画像をRGBからHSVに変換 + cv::cvtColor(cv_img->image, cv_img->image, cv::COLOR_RGB2HSV); + + // 画像処理用の変数を用意 + cv::Mat img_thresholded; + + // 画像の二値化 + cv::inRange( + cv_img->image, + 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::Moments moment = moments(img_thresholded); + double d_m01 = moment.m01; + double d_m10 = moment.m10; + double d_area = moment.m00; + + // 検出した領域のピクセル数が10000より大きい場合 + if (d_area > 10000) { + // 画像座標系における把持対象物の位置（2D） + const double pixel_x = d_m10 / d_area; + const double pixel_y = d_m01 / d_area; + const double pixel_err_x = pixel_x - msg->width / 2.0; + const double pixel_err_y = pixel_y - msg->height / 2.0; + const cv::Point2d point(pixel_err_x, pixel_err_y); + RCLCPP_INFO_STREAM(this->get_logger(), "Detect at" << point << "."); + + // 現在角度をベースに、目標角度を作成する + auto joint_values = move_group_neck_->getCurrentJointValues(); + + const double OPERATION_GAIN_X = 0.001; + // 首を左に向ける + joint_values[0] = joint_values[0] - pixel_err_x * OPERATION_GAIN_X; + move_group_neck_->setJointValueTarget(joint_values); + move_group_neck_->move(); + + // 閾値による二値化画像を配信 + sensor_msgs::msg::Image::SharedPtr img_thresholded_msg = + cv_bridge::CvImage(msg->header, "mono8", img_thresholded).toImageMsg(); + image_thresholded_publisher_->publish(*img_thresholded_msg); + } + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto move_group_neck_node = rclcpp::Node::make_shared("move_group_neck_node", node_options); + + rclcpp::executors::MultiThreadedExecutor exec; + auto head_camera_tracking_node = std::make_shared( + move_group_neck_node); + exec.add_node(head_camera_tracking_node); + exec.add_node(move_group_neck_node); + exec.spin(); + rclcpp::shutdown(); + return 0; +} From ef48959b1a2eb29a427ce60350a252776f3688d5 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Mon, 4 Dec 2023 17:06:51 +0900 Subject: [PATCH 03/55] =?UTF-8?q?=E3=82=AF=E3=83=A9=E3=82=B9=E3=81=AE?= =?UTF-8?q?=E5=88=86=E5=89=B2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/head_camera_tracking.cpp | 130 ++++++++++++++---- 1 file changed, 100 insertions(+), 30 deletions(-) diff --git a/sciurus17_examples/src/head_camera_tracking.cpp b/sciurus17_examples/src/head_camera_tracking.cpp index 76fe248..0eb4c2c 100644 --- a/sciurus17_examples/src/head_camera_tracking.cpp +++ b/sciurus17_examples/src/head_camera_tracking.cpp @@ -19,12 +19,14 @@ #include #include #include +#include "rclcpp_action/rclcpp_action.hpp" #include "moveit/move_group_interface/move_group_interface.h" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "sensor_msgs/msg/camera_info.hpp" #include "sensor_msgs/msg/image.hpp" +#include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "tf2/LinearMath/Quaternion.h" #include "tf2/LinearMath/Matrix3x3.h" #include "tf2_ros/transform_broadcaster.h" @@ -32,36 +34,37 @@ #include "opencv2/imgproc/imgproc.hpp" #include "cv_bridge/cv_bridge.h" #include "image_geometry/pinhole_camera_model.h" +#include "control_msgs/action/follow_joint_trajectory.hpp" using std::placeholders::_1; using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; +using namespace std::chrono_literals; class HeadCameraTracking : public rclcpp::Node { public: + using GoalHandle = rclcpp_action::ClientGoalHandle; + HeadCameraTracking( - rclcpp::Node::SharedPtr move_group_neck_node) + ) : Node("head_camera_tracking") { - move_group_neck_ = std::make_shared(move_group_neck_node, "neck_group"); - move_group_neck_->setMaxVelocityScalingFactor(0.5); - move_group_neck_->setMaxAccelerationScalingFactor(0.5); - - move_group_neck_->setNamedTarget("neck_init_pose"); - move_group_neck_->move(); - image_subscription_ = this->create_subscription( "/head_camera/color/image_raw", 10, std::bind(&HeadCameraTracking::image_callback, this, _1)); image_thresholded_publisher_ = this->create_publisher("image_thresholded", 10); + + timer_ = this->create_wall_timer( + 500ms, std::bind(HeadCameraTracking::tracking_callback, this)); } private: rclcpp::Subscription::SharedPtr image_subscription_; rclcpp::Publisher::SharedPtr image_thresholded_publisher_; - std::shared_ptr move_group_neck_; + rclcpp::TimerBase::SharedPtr timer_; + sensor_msgs::msg::Image::SharedPtr color_image_; - void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) + bool color_detection(cv::Point2d& pixel_err) { // オレンジ色の物体を検出するようにHSVの範囲を設定 // 周囲の明るさ等の動作環境に合わせて調整 @@ -70,7 +73,7 @@ class HeadCameraTracking : public rclcpp::Node const int LOW_V = 120, HIGH_V = 255; // ウェブカメラの画像を受け取る - auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); + auto cv_img = cv_bridge::toCvShare(color_image_, color_image_->encoding); // 画像をRGBからHSVに変換 cv::cvtColor(cv_img->image, cv_img->image, cv::COLOR_RGB2HSV); @@ -106,29 +109,97 @@ class HeadCameraTracking : public rclcpp::Node double d_area = moment.m00; // 検出した領域のピクセル数が10000より大きい場合 - if (d_area > 10000) { + bool object_detected = d_area > 10000; + + if (object_detected) { // 画像座標系における把持対象物の位置（2D） const double pixel_x = d_m10 / d_area; const double pixel_y = d_m01 / d_area; - const double pixel_err_x = pixel_x - msg->width / 2.0; - const double pixel_err_y = pixel_y - msg->height / 2.0; - const cv::Point2d point(pixel_err_x, pixel_err_y); - RCLCPP_INFO_STREAM(this->get_logger(), "Detect at" << point << "."); - - // 現在角度をベースに、目標角度を作成する - auto joint_values = move_group_neck_->getCurrentJointValues(); - - const double OPERATION_GAIN_X = 0.001; - // 首を左に向ける - joint_values[0] = joint_values[0] - pixel_err_x * OPERATION_GAIN_X; - move_group_neck_->setJointValueTarget(joint_values); - move_group_neck_->move(); + const double pixel_err_x = pixel_x - color_image_->width / 2.0; + const double pixel_err_y = pixel_y - color_image_->height / 2.0; + pixel_err = cv::Point2d(pixel_err_x, pixel_err_y); + RCLCPP_INFO_STREAM(this->get_logger(), "Detect at" << pixel_err << "."); // 閾値による二値化画像を配信 sensor_msgs::msg::Image::SharedPtr img_thresholded_msg = - cv_bridge::CvImage(msg->header, "mono8", img_thresholded).toImageMsg(); + cv_bridge::CvImage(color_image_->header, "mono8", img_thresholded).toImageMsg(); image_thresholded_publisher_->publish(*img_thresholded_msg); } + return object_detected; + } + + void tracking_callback() + { + auto neck = NeckControl(); + cv::Point2d pixel_err; + if(color_detection(pixel_err)) { + const double OPERATION_GAIN_X = 0.001; + auto yaw_angle = current_yaw_angle_ - pixel_err_x * OPERATION_GAIN_X; + neck.send_goal(yaw_angle, 0, 0.01); + } + } + + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) + { + color_image_ = msg; + } +}; + +class NeckControl : public rclcpp::Node +{ +public: + using GoalHandle = rclcpp_action::ClientGoalHandle; + + NeckControl( + ) + : Node("neck_control") + { + this->client_ptr_ = rclcpp_action::create_client( + this, + "neck_controller/follow_joint_trajectory"); + + state_subscription_ = this->create_subscription( + "/neck_controller/state", 10, std::bind(&NeckControl::state_callback, this, _1)); + } + + void send_goal(double yaw_angle, double pitch_angle, double seconds) + { + using namespace std::placeholders; + + if(!this->client_ptr_->wait_for_action_server(5s)) { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + } + + auto goal_msg = control_msgs::action::FollowJointTrajectory::Goal(); + goal_msg.trajectory.joint_names.push_back("neck_yaw_joint"); + goal_msg.trajectory.joint_names.push_back("neck_pitch_joint"); + + trajectory_msgs::msg::JointTrajectoryPoint trajectory_point_msg; + trajectory_point_msg.positions.push_back(yaw_angle); + trajectory_point_msg.positions.push_back(pitch_angle); + trajectory_point_msg.time_from_start = rclcpp::Duration::from_seconds(seconds); + goal_msg.trajectory.points.push_back(trajectory_point_msg); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.feedback_callback = + std::bind(&NeckControl::feedback_callback, this, _1, _2); + this->client_ptr_->async_send_goal(goal_msg, send_goal_options); + } + +private: + rclcpp_action::Client::SharedPtr client_ptr_; + rclcpp::Subscription::SharedPtr state_subscription_; + double current_yaw_angle_; + double current_pitch_angle_; + + void state_callback(const control_msgs::msg::JointTrajectoryControllerState) + void feedback_callback( + GoalHandle::SharedPtr, + const std::shared_ptr feedback) + { + current_yaw_angle_ = feedback->actual.positions[0]; + current_pitch_angle_ = feedback->actual.positions[1]; } }; @@ -137,13 +208,12 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true); - auto move_group_neck_node = rclcpp::Node::make_shared("move_group_neck_node", node_options); rclcpp::executors::MultiThreadedExecutor exec; - auto head_camera_tracking_node = std::make_shared( - move_group_neck_node); + auto head_camera_tracking_node = std::make_shared(); + auto neck_control_node = std::make_shared(); exec.add_node(head_camera_tracking_node); - exec.add_node(move_group_neck_node); + exec.add_node(neck_control_node); exec.spin(); rclcpp::shutdown(); return 0; From 9ba146e03807c99a4838317638be8254c43c20b7 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Tue, 5 Dec 2023 20:29:30 +0900 Subject: [PATCH 04/55] =?UTF-8?q?=E3=83=88=E3=83=A9=E3=83=83=E3=82=AD?= =?UTF-8?q?=E3=83=B3=E3=82=B0=E5=8B=95=E4=BD=9C=E3=82=92main=E9=96=A2?= =?UTF-8?q?=E6=95=B0=E3=81=AB=E7=A7=BB=E5=8B=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/head_camera_tracking.cpp | 212 +++++++++++++----- 1 file changed, 150 insertions(+), 62 deletions(-) diff --git a/sciurus17_examples/src/head_camera_tracking.cpp b/sciurus17_examples/src/head_camera_tracking.cpp index 0eb4c2c..20ebafd 100644 --- a/sciurus17_examples/src/head_camera_tracking.cpp +++ b/sciurus17_examples/src/head_camera_tracking.cpp @@ -20,6 +20,7 @@ #include #include #include "rclcpp_action/rclcpp_action.hpp" +#include "angles/angles.h" #include "moveit/move_group_interface/move_group_interface.h" #include "rclcpp/rclcpp.hpp" @@ -39,32 +40,40 @@ using std::placeholders::_1; using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; using namespace std::chrono_literals; -class HeadCameraTracking : public rclcpp::Node +class ObjectTracker : public rclcpp::Node { public: - using GoalHandle = rclcpp_action::ClientGoalHandle; - HeadCameraTracking( - ) - : Node("head_camera_tracking") + ObjectTracker() + : Node("head_camera_tracking_2") { image_subscription_ = this->create_subscription( - "/head_camera/color/image_raw", 10, std::bind(&HeadCameraTracking::image_callback, this, _1)); + "/head_camera/color/image_raw", 10, std::bind(&ObjectTracker::image_callback, this, _1)); image_thresholded_publisher_ = this->create_publisher("image_thresholded", 10); + } - timer_ = this->create_wall_timer( - 500ms, std::bind(HeadCameraTracking::tracking_callback, this)); + cv::Point2d get_normalized_object_point() + { + return normalized_object_point_; + } + + bool has_object_point() + { + return has_object_point_; } private: rclcpp::Subscription::SharedPtr image_subscription_; rclcpp::Publisher::SharedPtr image_thresholded_publisher_; rclcpp::TimerBase::SharedPtr timer_; - sensor_msgs::msg::Image::SharedPtr color_image_; + cv::Point2d pixel_err_; + bool has_object_point_ = false; + // -1.0 ~ 1.0に正規化された検出位置 + cv::Point2d normalized_object_point_; - bool color_detection(cv::Point2d& pixel_err) + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { // オレンジ色の物体を検出するようにHSVの範囲を設定 // 周囲の明るさ等の動作環境に合わせて調整 @@ -73,7 +82,7 @@ class HeadCameraTracking : public rclcpp::Node const int LOW_V = 120, HIGH_V = 255; // ウェブカメラの画像を受け取る - auto cv_img = cv_bridge::toCvShare(color_image_, color_image_->encoding); + auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); // 画像をRGBからHSVに変換 cv::cvtColor(cv_img->image, cv_img->image, cv::COLOR_RGB2HSV); @@ -102,56 +111,46 @@ class HeadCameraTracking : public rclcpp::Node cv::MORPH_CLOSE, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); + // 閾値による二値化画像を配信 + sensor_msgs::msg::Image::SharedPtr img_thresholded_msg = + cv_bridge::CvImage(msg->header, "mono8", img_thresholded).toImageMsg(); + image_thresholded_publisher_->publish(*img_thresholded_msg); + // 画像の検出領域におけるモーメントを計算 cv::Moments moment = moments(img_thresholded); double d_m01 = moment.m01; double d_m10 = moment.m10; double d_area = moment.m00; - // 検出した領域のピクセル数が10000より大きい場合 - bool object_detected = d_area > 10000; - - if (object_detected) { - // 画像座標系における把持対象物の位置（2D） - const double pixel_x = d_m10 / d_area; - const double pixel_y = d_m01 / d_area; - const double pixel_err_x = pixel_x - color_image_->width / 2.0; - const double pixel_err_y = pixel_y - color_image_->height / 2.0; - pixel_err = cv::Point2d(pixel_err_x, pixel_err_y); - RCLCPP_INFO_STREAM(this->get_logger(), "Detect at" << pixel_err << "."); - - // 閾値による二値化画像を配信 - sensor_msgs::msg::Image::SharedPtr img_thresholded_msg = - cv_bridge::CvImage(color_image_->header, "mono8", img_thresholded).toImageMsg(); - image_thresholded_publisher_->publish(*img_thresholded_msg); - } - return object_detected; - } + // 検出領域のピクセル数が100000より大きい場合 + has_object_point_ = d_area > 100000; - void tracking_callback() - { - auto neck = NeckControl(); - cv::Point2d pixel_err; - if(color_detection(pixel_err)) { - const double OPERATION_GAIN_X = 0.001; - auto yaw_angle = current_yaw_angle_ - pixel_err_x * OPERATION_GAIN_X; - neck.send_goal(yaw_angle, 0, 0.01); - } - } + if (has_object_point_) { + // 画像座標系における物体検出位置（2D） + cv::Point2d object_point; + object_point.x = d_m10 / d_area; + object_point.y = d_m01 / d_area; - void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) - { - color_image_ = msg; + RCLCPP_INFO_STREAM(this->get_logger(), "Detect at" << object_point << "."); + + // 画像の中心を原点とした検出位置に変換 + 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; + + if (msg->width != 0 && msg->height) { + normalized_object_point_.x = translated_object_point.x / msg->width / 2.0; + normalized_object_point_.y = translated_object_point.y / msg->height / 2.0; + } + } } }; class NeckControl : public rclcpp::Node { public: - using GoalHandle = rclcpp_action::ClientGoalHandle; - NeckControl( - ) + NeckControl() : Node("neck_control") { this->client_ptr_ = rclcpp_action::create_client( @@ -159,7 +158,7 @@ class NeckControl : public rclcpp::Node "neck_controller/follow_joint_trajectory"); state_subscription_ = this->create_subscription( - "/neck_controller/state", 10, std::bind(&NeckControl::state_callback, this, _1)); + "/neck_controller/controller_state", 10, std::bind(&NeckControl::state_callback, this, _1)); } void send_goal(double yaw_angle, double pitch_angle, double seconds) @@ -181,25 +180,37 @@ class NeckControl : public rclcpp::Node trajectory_point_msg.time_from_start = rclcpp::Duration::from_seconds(seconds); goal_msg.trajectory.points.push_back(trajectory_point_msg); - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.feedback_callback = - std::bind(&NeckControl::feedback_callback, this, _1, _2); - this->client_ptr_->async_send_goal(goal_msg, send_goal_options); + this->client_ptr_->async_send_goal(goal_msg); + } + + double get_current_yaw_angle() + { + return current_yaw_angle_; + } + + double get_current_pitch_angle() + { + return current_pitch_angle_; + } + + bool has_state() + { + return has_state_; } private: rclcpp_action::Client::SharedPtr client_ptr_; rclcpp::Subscription::SharedPtr state_subscription_; - double current_yaw_angle_; - double current_pitch_angle_; + control_msgs::msg::JointTrajectoryControllerState::SharedPtr neck_state_; + double current_yaw_angle_ = 0; + double current_pitch_angle_ = 0; + bool has_state_ = false; - void state_callback(const control_msgs::msg::JointTrajectoryControllerState) - void feedback_callback( - GoalHandle::SharedPtr, - const std::shared_ptr feedback) + void state_callback(const control_msgs::msg::JointTrajectoryControllerState::SharedPtr msg) { - current_yaw_angle_ = feedback->actual.positions[0]; - current_pitch_angle_ = feedback->actual.positions[1]; + current_yaw_angle_ = msg->feedback.positions[0]; + current_pitch_angle_ = msg->feedback.positions[1]; + has_state_ = true; } }; @@ -207,14 +218,91 @@ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; + rclcpp::WallRate loop_rate(500ms); node_options.automatically_declare_parameters_from_overrides(true); + const double THRESH_X = 0.05; + const double THRESH_Y= 0.05; + + const double INITIAL_YAW_ANGLE = 0; + const double INITIAL_PITCH_ANGLE = 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(-70); + + const double RESET_ANGLE_VEL = angles::from_degrees(5); + + const std::chrono::nanoseconds DETECTION_TIMEOUT = 3s; + + const double OPERATION_GAIN_X = 0.5; + const double OPERATION_GAIN_Y = 0.5; + + double yaw_angle = 0; + double pitch_angle = 0; + rclcpp::executors::MultiThreadedExecutor exec; - auto head_camera_tracking_node = std::make_shared(); + auto objeckt_tracker_node = std::make_shared(); auto neck_control_node = std::make_shared(); - exec.add_node(head_camera_tracking_node); + exec.add_node(objeckt_tracker_node); exec.add_node(neck_control_node); - exec.spin(); + rclcpp::Time detection_timestamp; + bool look_object = false; + yaw_angle = neck_control_node->get_current_yaw_angle(); + pitch_angle = neck_control_node->get_current_pitch_angle(); + while (rclcpp::ok()) { + if (objeckt_tracker_node->has_object_point() && neck_control_node->has_state()) { + detection_timestamp = objeckt_tracker_node->get_clock()->now(); + look_object = true; + } else { + auto lost_time = objeckt_tracker_node->get_clock()->now().nanoseconds() - detection_timestamp.nanoseconds(); + if (lost_time < DETECTION_TIMEOUT.count()) { + look_object = false; + } + } + // 物体が検出されていて、現在の首角度が取得できている場合に物体追従を行う + if (look_object) { + + // 現在の首角度を取得 + auto current_yaw_angle = neck_control_node->get_current_yaw_angle(); + auto current_pitch_angle = neck_control_node->get_current_pitch_angle(); + + // 物体検出位置を取得 + auto object_point = objeckt_tracker_node->get_normalized_object_point(); + + // 物体検出位置が閾値以上の場合、目標首角度を計算 + if (std::abs(object_point.x) > THRESH_X) { + yaw_angle -= object_point.x * OPERATION_GAIN_X; + } + if (std::abs(object_point.y) > THRESH_Y) { + pitch_angle -= object_point.y * OPERATION_GAIN_Y; + } + + // 目標首角度を制限角度内に収める + yaw_angle = std::clamp(yaw_angle, MIN_YAW_ANGLE, MAX_YAW_ANGLE); + pitch_angle = std::clamp(pitch_angle, MIN_PITCH_ANGLE, MAX_PITCH_ANGLE); + + // 目標角度に首を動かす + neck_control_node->send_goal(yaw_angle, pitch_angle, 0.1); + } else { + auto diff_yaw_angle = INITIAL_YAW_ANGLE - yaw_angle; + if (std::abs(diff_yaw_angle) > RESET_ANGLE_VEL) { + yaw_angle += std::copysign(RESET_ANGLE_VEL, diff_yaw_angle); + } else { + yaw_angle = INITIAL_YAW_ANGLE; + } + auto diff_pitch_angle = INITIAL_PITCH_ANGLE - pitch_angle; + if (std::abs(diff_pitch_angle) > RESET_ANGLE_VEL) { + pitch_angle += std::copysign(RESET_ANGLE_VEL, diff_pitch_angle); + } else { + pitch_angle = INITIAL_PITCH_ANGLE; + } + neck_control_node->send_goal(yaw_angle, pitch_angle, 0.1); + } + exec.spin_once(); + loop_rate.sleep(); + } rclcpp::shutdown(); return 0; } From a781ef2f84ae9c9092b3e3a3efecb859e8019d9e Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Tue, 5 Dec 2023 20:30:10 +0900 Subject: [PATCH 05/55] =?UTF-8?q?=E4=BE=9D=E5=AD=98=E3=83=91=E3=83=83?= =?UTF-8?q?=E3=82=B1=E3=83=BC=E3=82=B8=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index e696b3e..cded4a5 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/sciurus17_examples/CMakeLists.txt @@ -15,11 +15,14 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +add_compile_options(-g) + # 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) @@ -27,6 +30,7 @@ find_package(pcl_conversions REQUIRED) find_package(pcl_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(rclcpp_action REQUIRED) set(LIBRARY_NAME "pose_presets") add_library(${LIBRARY_NAME} @@ -79,6 +83,7 @@ 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 @@ -88,6 +93,7 @@ foreach(loop_var IN LISTS executable_list) pcl_conversions rclcpp tf2_geometry_msgs + control_msgs ) install(TARGETS From c2d436f5b7a69c3d27f2527a31fce02437c1ba29 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Wed, 6 Dec 2023 13:46:44 +0900 Subject: [PATCH 06/55] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AA=E8=A1=8C?= =?UTF-8?q?=E3=81=AE=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index cded4a5..1271147 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/sciurus17_examples/CMakeLists.txt @@ -15,8 +15,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -add_compile_options(-g) - # find dependencies find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) From 3db140c97669a43a79ef5cfc5ebd7a6eed35b168 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Wed, 6 Dec 2023 15:50:24 +0900 Subject: [PATCH 07/55] =?UTF-8?q?=E3=82=B3=E3=83=A1=E3=83=B3=E3=83=88?= =?UTF-8?q?=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/head_camera_tracking.cpp | 82 +++++++++++-------- 1 file changed, 50 insertions(+), 32 deletions(-) diff --git a/sciurus17_examples/src/head_camera_tracking.cpp b/sciurus17_examples/src/head_camera_tracking.cpp index 20ebafd..8f6cd38 100644 --- a/sciurus17_examples/src/head_camera_tracking.cpp +++ b/sciurus17_examples/src/head_camera_tracking.cpp @@ -218,88 +218,106 @@ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; - rclcpp::WallRate loop_rate(500ms); + rclcpp::WallRate loop_rate(100ms); node_options.automatically_declare_parameters_from_overrides(true); + rclcpp::executors::MultiThreadedExecutor exec; + auto objeckt_tracker_node = std::make_shared(); + auto neck_control_node = std::make_shared(); + exec.add_node(objeckt_tracker_node); + exec.add_node(neck_control_node); + exec.spin_once(); + + // 追従を開始する物体位置の閾値 const double THRESH_X = 0.05; const double THRESH_Y= 0.05; + // 首角度の初期値 const double INITIAL_YAW_ANGLE = 0; const double INITIAL_PITCH_ANGLE = 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(-70); - const double RESET_ANGLE_VEL = angles::from_degrees(5); + // 首角度初期化時の制御角度 + const double RESET_ANGLE_VEL = angles::from_degrees(3); - const std::chrono::nanoseconds DETECTION_TIMEOUT = 3s; + // 物体が検出されなくなってから初期角度に戻り始めるまでの時間 + const std::chrono::nanoseconds DETECTION_TIMEOUT = 1s; + // 首角度制御量 + // 値が大きいほど追従速度が速くなる const double OPERATION_GAIN_X = 0.5; const double OPERATION_GAIN_Y = 0.5; - double yaw_angle = 0; - double pitch_angle = 0; + // 物体検出時の時刻 + auto detection_timestamp = objeckt_tracker_node->get_clock()->now().nanoseconds(); - rclcpp::executors::MultiThreadedExecutor exec; - auto objeckt_tracker_node = std::make_shared(); - auto neck_control_node = std::make_shared(); - exec.add_node(objeckt_tracker_node); - exec.add_node(neck_control_node); - rclcpp::Time detection_timestamp; + // 追従動作開始フラグ bool look_object = false; - yaw_angle = neck_control_node->get_current_yaw_angle(); - pitch_angle = neck_control_node->get_current_pitch_angle(); + + // 現在の首角度を取得 + while (!neck_control_node->has_state()) { + exec.spin_once(); + } + auto yaw_angle = neck_control_node->get_current_yaw_angle(); + auto pitch_angle = neck_control_node->get_current_pitch_angle(); + while (rclcpp::ok()) { - if (objeckt_tracker_node->has_object_point() && neck_control_node->has_state()) { - detection_timestamp = objeckt_tracker_node->get_clock()->now(); + // 現在時刻 + auto now = objeckt_tracker_node->get_clock()->now().nanoseconds(); + + // 物体を検出したとき追従動作開始フラグをtrueにする + if (objeckt_tracker_node->has_object_point()) { + detection_timestamp = now; look_object = true; } else { - auto lost_time = objeckt_tracker_node->get_clock()->now().nanoseconds() - detection_timestamp.nanoseconds(); - if (lost_time < DETECTION_TIMEOUT.count()) { + // 物体を検出しなくなってから1秒後に初期角度へ戻る + auto lost_time = now - detection_timestamp; + if (lost_time > DETECTION_TIMEOUT.count()) { look_object = false; } } - // 物体が検出されていて、現在の首角度が取得できている場合に物体追従を行う - if (look_object) { - - // 現在の首角度を取得 - auto current_yaw_angle = neck_control_node->get_current_yaw_angle(); - auto current_pitch_angle = neck_control_node->get_current_pitch_angle(); + // 物体が検出されたら追従を行う + if (look_object) { // 物体検出位置を取得 auto object_point = objeckt_tracker_node->get_normalized_object_point(); - // 物体検出位置が閾値以上の場合、目標首角度を計算 + // 追従動作のための首角度を計算 if (std::abs(object_point.x) > THRESH_X) { yaw_angle -= object_point.x * OPERATION_GAIN_X; } if (std::abs(object_point.y) > THRESH_Y) { pitch_angle -= object_point.y * OPERATION_GAIN_Y; } - - // 目標首角度を制限角度内に収める - yaw_angle = std::clamp(yaw_angle, MIN_YAW_ANGLE, MAX_YAW_ANGLE); - pitch_angle = std::clamp(pitch_angle, MIN_PITCH_ANGLE, MAX_PITCH_ANGLE); - - // 目標角度に首を動かす - neck_control_node->send_goal(yaw_angle, pitch_angle, 0.1); } else { + // ゆっくりと初期角度へ戻る auto diff_yaw_angle = INITIAL_YAW_ANGLE - yaw_angle; if (std::abs(diff_yaw_angle) > RESET_ANGLE_VEL) { yaw_angle += std::copysign(RESET_ANGLE_VEL, diff_yaw_angle); } else { yaw_angle = INITIAL_YAW_ANGLE; } + auto diff_pitch_angle = INITIAL_PITCH_ANGLE - pitch_angle; if (std::abs(diff_pitch_angle) > RESET_ANGLE_VEL) { pitch_angle += std::copysign(RESET_ANGLE_VEL, diff_pitch_angle); } else { pitch_angle = INITIAL_PITCH_ANGLE; } - neck_control_node->send_goal(yaw_angle, pitch_angle, 0.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); + + // 目標角度に首を動かす + neck_control_node->send_goal(yaw_angle, pitch_angle, 0.1); + exec.spin_once(); loop_rate.sleep(); } From d07381ea2c58af69ad118dd5c644142de3f0c4d2 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Wed, 6 Dec 2023 19:44:28 +0900 Subject: [PATCH 08/55] =?UTF-8?q?=E5=88=B6=E5=BE=A1=E5=91=A8=E6=9C=9F?= =?UTF-8?q?=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/head_camera_tracking.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/sciurus17_examples/src/head_camera_tracking.cpp b/sciurus17_examples/src/head_camera_tracking.cpp index 8f6cd38..239c8f8 100644 --- a/sciurus17_examples/src/head_camera_tracking.cpp +++ b/sciurus17_examples/src/head_camera_tracking.cpp @@ -218,7 +218,7 @@ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; - rclcpp::WallRate loop_rate(100ms); + rclcpp::WallRate loop_rate(20ms); node_options.automatically_declare_parameters_from_overrides(true); rclcpp::executors::MultiThreadedExecutor exec; @@ -240,18 +240,18 @@ int main(int argc, char * argv[]) 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(-70); + const double MIN_PITCH_ANGLE = angles::from_degrees(-85); // 首角度初期化時の制御角度 - const double RESET_ANGLE_VEL = angles::from_degrees(3); + const double RESET_ANGLE_VEL = angles::from_degrees(0.3); // 物体が検出されなくなってから初期角度に戻り始めるまでの時間 const std::chrono::nanoseconds DETECTION_TIMEOUT = 1s; // 首角度制御量 // 値が大きいほど追従速度が速くなる - const double OPERATION_GAIN_X = 0.5; - const double OPERATION_GAIN_Y = 0.5; + const double OPERATION_GAIN_X = 0.05; + const double OPERATION_GAIN_Y = 0.05; // 物体検出時の時刻 auto detection_timestamp = objeckt_tracker_node->get_clock()->now().nanoseconds(); @@ -316,7 +316,7 @@ int main(int argc, char * argv[]) pitch_angle = std::clamp(pitch_angle, MIN_PITCH_ANGLE, MAX_PITCH_ANGLE); // 目標角度に首を動かす - neck_control_node->send_goal(yaw_angle, pitch_angle, 0.1); + neck_control_node->send_goal(yaw_angle, pitch_angle, 0.01); exec.spin_once(); loop_rate.sleep(); From 49f4c90b61b2678601d1a9d3d14967163740a94f Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Thu, 7 Dec 2023 19:15:26 +0900 Subject: [PATCH 09/55] =?UTF-8?q?=E8=BF=BD=E5=BE=93=E5=87=A6=E7=90=86?= =?UTF-8?q?=E3=82=92=E3=82=AF=E3=83=A9=E3=82=B9=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/head_camera_tracking.cpp | 298 +++++++++--------- 1 file changed, 157 insertions(+), 141 deletions(-) diff --git a/sciurus17_examples/src/head_camera_tracking.cpp b/sciurus17_examples/src/head_camera_tracking.cpp index 239c8f8..8f21224 100644 --- a/sciurus17_examples/src/head_camera_tracking.cpp +++ b/sciurus17_examples/src/head_camera_tracking.cpp @@ -22,51 +22,39 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "angles/angles.h" -#include "moveit/move_group_interface/move_group_interface.h" +#include "std_msgs/msg/float64_multi_array.hpp" #include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" #include "sensor_msgs/msg/camera_info.hpp" #include "sensor_msgs/msg/image.hpp" #include "control_msgs/msg/joint_trajectory_controller_state.hpp" -#include "tf2/LinearMath/Quaternion.h" -#include "tf2/LinearMath/Matrix3x3.h" -#include "tf2_ros/transform_broadcaster.h" #include "opencv2/opencv.hpp" -#include "opencv2/imgproc/imgproc.hpp" #include "cv_bridge/cv_bridge.h" -#include "image_geometry/pinhole_camera_model.h" #include "control_msgs/action/follow_joint_trajectory.hpp" using std::placeholders::_1; -using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; using namespace std::chrono_literals; -class ObjectTracker : public rclcpp::Node +class ColorDetection : public rclcpp::Node { public: - ObjectTracker() - : Node("head_camera_tracking_2") + ColorDetection() + : Node("color_detection") { image_subscription_ = this->create_subscription( - "/head_camera/color/image_raw", 10, std::bind(&ObjectTracker::image_callback, this, _1)); + "/head_camera/color/image_raw", 10, std::bind(&ColorDetection::image_callback, this, _1)); image_thresholded_publisher_ = this->create_publisher("image_thresholded", 10); - } - - cv::Point2d get_normalized_object_point() - { - return normalized_object_point_; - } - bool has_object_point() - { - return has_object_point_; + object_point_publisher_ = + this->create_publisher("object_detected", 10); } private: rclcpp::Subscription::SharedPtr image_subscription_; rclcpp::Publisher::SharedPtr image_thresholded_publisher_; + rclcpp::Publisher::SharedPtr object_point_publisher_; rclcpp::TimerBase::SharedPtr timer_; cv::Point2d pixel_err_; bool has_object_point_ = false; @@ -142,185 +130,213 @@ class ObjectTracker : public rclcpp::Node 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); } } }; -class NeckControl : public rclcpp::Node +class ObjectTracker : public rclcpp::Node { public: - NeckControl() - : Node("neck_control") + ObjectTracker() + : Node("object_tracker") { - this->client_ptr_ = rclcpp_action::create_client( - this, - "neck_controller/follow_joint_trajectory"); + timer_ = this->create_wall_timer( + 20ms, std::bind(&ObjectTracker::tracking, this)); state_subscription_ = this->create_subscription( - "/neck_controller/controller_state", 10, std::bind(&NeckControl::state_callback, this, _1)); - } + "/neck_controller/controller_state", 10, std::bind(&ObjectTracker::state_callback, this, _1)); - void send_goal(double yaw_angle, double pitch_angle, double seconds) - { - using namespace std::placeholders; - - if(!this->client_ptr_->wait_for_action_server(5s)) { - RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); - rclcpp::shutdown(); - } + object_point_subscription_ = this->create_subscription( + "/object_detected", 10, std::bind(&ObjectTracker::point_callback, this, _1)); - auto goal_msg = control_msgs::action::FollowJointTrajectory::Goal(); - goal_msg.trajectory.joint_names.push_back("neck_yaw_joint"); - goal_msg.trajectory.joint_names.push_back("neck_pitch_joint"); - - trajectory_msgs::msg::JointTrajectoryPoint trajectory_point_msg; - trajectory_point_msg.positions.push_back(yaw_angle); - trajectory_point_msg.positions.push_back(pitch_angle); - trajectory_point_msg.time_from_start = rclcpp::Duration::from_seconds(seconds); - goal_msg.trajectory.points.push_back(trajectory_point_msg); - - this->client_ptr_->async_send_goal(goal_msg); - } - - double get_current_yaw_angle() - { - return current_yaw_angle_; - } - - double get_current_pitch_angle() - { - return current_pitch_angle_; - } - - bool has_state() - { - return has_state_; + angles_publisher_ = + this->create_publisher("/target_angles", 10); } private: - rclcpp_action::Client::SharedPtr client_ptr_; + rclcpp::TimerBase::SharedPtr timer_; + cv::Point2d pixel_err_; + // -1.0 ~ 1.0に正規化された検出位置 + cv::Point2d normalized_object_point_; rclcpp::Subscription::SharedPtr state_subscription_; - control_msgs::msg::JointTrajectoryControllerState::SharedPtr neck_state_; + rclcpp::Subscription::SharedPtr object_point_subscription_; + rclcpp::Publisher::SharedPtr angles_publisher_; double current_yaw_angle_ = 0; double current_pitch_angle_ = 0; - bool has_state_ = false; + std::vector current_angles_; + geometry_msgs::msg::PointStamped::SharedPtr object_point_msg_; + double yaw_angle_ = 0; + double pitch_angle_ = 0; + std::vector target_angles_; void state_callback(const control_msgs::msg::JointTrajectoryControllerState::SharedPtr msg) { - current_yaw_angle_ = msg->feedback.positions[0]; - current_pitch_angle_ = msg->feedback.positions[1]; - has_state_ = true; + current_angles_ = msg->feedback.positions; } -}; -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - rclcpp::WallRate loop_rate(20ms); - node_options.automatically_declare_parameters_from_overrides(true); - - rclcpp::executors::MultiThreadedExecutor exec; - auto objeckt_tracker_node = std::make_shared(); - auto neck_control_node = std::make_shared(); - exec.add_node(objeckt_tracker_node); - exec.add_node(neck_control_node); - exec.spin_once(); - - // 追従を開始する物体位置の閾値 - const double THRESH_X = 0.05; - const double THRESH_Y= 0.05; + void point_callback(const geometry_msgs::msg::PointStamped::SharedPtr msg) + { + object_point_msg_ = msg; + } - // 首角度の初期値 - const double INITIAL_YAW_ANGLE = 0; - const double INITIAL_PITCH_ANGLE = 0; + void tracking() + { + // 追従を開始する物体位置の閾値 + const double THRESH_X = 0.05; + const double THRESH_Y= 0.05; - // 首可動範囲 - 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(-85); + // 首角度の初期値 + const double INITIAL_YAW_ANGLE = 0; + const double INITIAL_PITCH_ANGLE = 0; - // 首角度初期化時の制御角度 - const double RESET_ANGLE_VEL = angles::from_degrees(0.3); + // 首可動範囲 + 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(-85); - // 物体が検出されなくなってから初期角度に戻り始めるまでの時間 - const std::chrono::nanoseconds DETECTION_TIMEOUT = 1s; + // 首角度初期化時の制御角度 + const double RESET_ANGLE_VEL = angles::from_degrees(0.3); - // 首角度制御量 - // 値が大きいほど追従速度が速くなる - const double OPERATION_GAIN_X = 0.05; - const double OPERATION_GAIN_Y = 0.05; + // 物体が検出されなくなってから初期角度に戻り始めるまでの時間 + const std::chrono::nanoseconds DETECTION_TIMEOUT = 3s; - // 物体検出時の時刻 - auto detection_timestamp = objeckt_tracker_node->get_clock()->now().nanoseconds(); + // 首角度制御量 + // 値が大きいほど追従速度が速くなる + const double OPERATION_GAIN_X = 0.05; + const double OPERATION_GAIN_Y = 0.05; - // 追従動作開始フラグ - bool look_object = false; + // 追従動作開始フラグ + bool look_object = false; - // 現在の首角度を取得 - while (!neck_control_node->has_state()) { - exec.spin_once(); - } - auto yaw_angle = neck_control_node->get_current_yaw_angle(); - auto pitch_angle = neck_control_node->get_current_pitch_angle(); + // 現在の首角度を取得 + if (current_angles_.empty()) { + return; + } + if (target_angles_.empty()) { + target_angles_ = current_angles_; + yaw_angle_ = current_angles_[0]; + pitch_angle_ = current_angles_[1]; + } - while (rclcpp::ok()) { // 現在時刻 - auto now = objeckt_tracker_node->get_clock()->now().nanoseconds(); + auto now = this->get_clock()->now().nanoseconds(); - // 物体を検出したとき追従動作開始フラグをtrueにする - if (objeckt_tracker_node->has_object_point()) { - detection_timestamp = now; - look_object = true; - } else { - // 物体を検出しなくなってから1秒後に初期角度へ戻る - auto lost_time = now - detection_timestamp; - if (lost_time > DETECTION_TIMEOUT.count()) { - look_object = false; - } + 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) { // 物体検出位置を取得 - auto object_point = objeckt_tracker_node->get_normalized_object_point(); + auto object_position = object_point_msg_->point; // 追従動作のための首角度を計算 - if (std::abs(object_point.x) > THRESH_X) { - yaw_angle -= object_point.x * OPERATION_GAIN_X; + if (std::abs(object_position.x) > THRESH_X) { + yaw_angle_ -= object_position.x * OPERATION_GAIN_X; } - if (std::abs(object_point.y) > THRESH_Y) { - pitch_angle -= object_point.y * OPERATION_GAIN_Y; + if (std::abs(object_position.y) > THRESH_Y) { + pitch_angle_ -= object_position.y * OPERATION_GAIN_Y; } } else { // ゆっくりと初期角度へ戻る - auto diff_yaw_angle = INITIAL_YAW_ANGLE - yaw_angle; + auto diff_yaw_angle = INITIAL_YAW_ANGLE - yaw_angle_; if (std::abs(diff_yaw_angle) > RESET_ANGLE_VEL) { - yaw_angle += std::copysign(RESET_ANGLE_VEL, diff_yaw_angle); + yaw_angle_ += std::copysign(RESET_ANGLE_VEL, diff_yaw_angle); } else { - yaw_angle = INITIAL_YAW_ANGLE; + yaw_angle_ = INITIAL_YAW_ANGLE; } - auto diff_pitch_angle = INITIAL_PITCH_ANGLE - pitch_angle; + auto diff_pitch_angle = INITIAL_PITCH_ANGLE - pitch_angle_; if (std::abs(diff_pitch_angle) > RESET_ANGLE_VEL) { - pitch_angle += std::copysign(RESET_ANGLE_VEL, diff_pitch_angle); + pitch_angle_ += std::copysign(RESET_ANGLE_VEL, diff_pitch_angle); } else { - pitch_angle = INITIAL_PITCH_ANGLE; + pitch_angle_ = INITIAL_PITCH_ANGLE; } } // 目標首角度を制限角度内に収める - yaw_angle = std::clamp(yaw_angle, MIN_YAW_ANGLE, MAX_YAW_ANGLE); - pitch_angle = std::clamp(pitch_angle, MIN_PITCH_ANGLE, MAX_PITCH_ANGLE); + yaw_angle_ = std::clamp(yaw_angle_, MIN_YAW_ANGLE, MAX_YAW_ANGLE); + pitch_angle_ = std::clamp(pitch_angle_, MIN_PITCH_ANGLE, MAX_PITCH_ANGLE); // 目標角度に首を動かす - neck_control_node->send_goal(yaw_angle, pitch_angle, 0.01); + std_msgs::msg::Float64MultiArray target_angles_msg; + target_angles_msg.data.push_back(yaw_angle_); + target_angles_msg.data.push_back(pitch_angle_); + angles_publisher_->publish(target_angles_msg); + } +}; + +class NeckControl : public rclcpp::Node +{ +public: + + NeckControl() + : Node("neck_control") + { + this->client_ptr_ = rclcpp_action::create_client( + this, + "neck_controller/follow_joint_trajectory"); + + angles_subscription_ = this->create_subscription( + "/target_angles", 10, std::bind(&NeckControl::angles_callback, this, _1)); + } + +private: + rclcpp_action::Client::SharedPtr client_ptr_; + rclcpp::Subscription::SharedPtr angles_subscription_; + control_msgs::msg::JointTrajectoryControllerState::SharedPtr neck_state_; + + void angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) { + using namespace std::placeholders; + auto yaw_angle = msg->data[0]; + auto pitch_angle = msg->data[1]; + const double TIME_FROM_START = 0.001; - exec.spin_once(); - loop_rate.sleep(); + if(!this->client_ptr_->wait_for_action_server(5s)) { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + } + + auto goal_msg = control_msgs::action::FollowJointTrajectory::Goal(); + goal_msg.trajectory.joint_names.push_back("neck_yaw_joint"); + goal_msg.trajectory.joint_names.push_back("neck_pitch_joint"); + + trajectory_msgs::msg::JointTrajectoryPoint trajectory_point_msg; + trajectory_point_msg.positions.push_back(yaw_angle); + trajectory_point_msg.positions.push_back(pitch_angle); + trajectory_point_msg.time_from_start = rclcpp::Duration::from_seconds(TIME_FROM_START); + goal_msg.trajectory.points.push_back(trajectory_point_msg); + + this->client_ptr_->async_send_goal(goal_msg); } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + rclcpp::WallRate loop_rate(20ms); + node_options.automatically_declare_parameters_from_overrides(true); + + rclcpp::executors::MultiThreadedExecutor exec; + auto color_detection_node = std::make_shared(); + auto neck_control_node = std::make_shared(); + auto objeckt_tracker_node = std::make_shared(); + exec.add_node(color_detection_node); + exec.add_node(neck_control_node); + exec.add_node(objeckt_tracker_node); + exec.spin(); + rclcpp::shutdown(); return 0; } From 8459f5f8f4aa68f34204852c33953441a708bb1e Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Fri, 8 Dec 2023 14:50:08 +0900 Subject: [PATCH 10/55] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AA=E5=A4=89?= =?UTF-8?q?=E6=95=B0=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/head_camera_tracking.cpp | 49 +++++++++---------- 1 file changed, 24 insertions(+), 25 deletions(-) diff --git a/sciurus17_examples/src/head_camera_tracking.cpp b/sciurus17_examples/src/head_camera_tracking.cpp index 8f21224..0534bd9 100644 --- a/sciurus17_examples/src/head_camera_tracking.cpp +++ b/sciurus17_examples/src/head_camera_tracking.cpp @@ -167,17 +167,13 @@ class ObjectTracker : public rclcpp::Node rclcpp::Subscription::SharedPtr state_subscription_; rclcpp::Subscription::SharedPtr object_point_subscription_; rclcpp::Publisher::SharedPtr angles_publisher_; - double current_yaw_angle_ = 0; - double current_pitch_angle_ = 0; - std::vector current_angles_; + control_msgs::msg::JointTrajectoryControllerState::SharedPtr current_angles_msg_; geometry_msgs::msg::PointStamped::SharedPtr object_point_msg_; - double yaw_angle_ = 0; - double pitch_angle_ = 0; std::vector target_angles_; void state_callback(const control_msgs::msg::JointTrajectoryControllerState::SharedPtr msg) { - current_angles_ = msg->feedback.positions; + current_angles_msg_ = msg; } void point_callback(const geometry_msgs::msg::PointStamped::SharedPtr msg) @@ -188,8 +184,8 @@ class ObjectTracker : public rclcpp::Node void tracking() { // 追従を開始する物体位置の閾値 - const double THRESH_X = 0.05; - const double THRESH_Y= 0.05; + const double THRESH_X = 0.03; + const double THRESH_Y= 0.03; // 首角度の初期値 const double INITIAL_YAW_ANGLE = 0; @@ -216,13 +212,16 @@ class ObjectTracker : public rclcpp::Node bool look_object = false; // 現在の首角度を取得 - if (current_angles_.empty()) { + if (!current_angles_msg_) { + RCLCPP_INFO_STREAM(this->get_logger(), "Wating controller state."); return; } + if (current_angles_msg_->feedback.positions.size() != 2) { + return; + } + const auto current_angles = current_angles_msg_->feedback.positions; if (target_angles_.empty()) { - target_angles_ = current_angles_; - yaw_angle_ = current_angles_[0]; - pitch_angle_ = current_angles_[1]; + target_angles_ = current_angles; } // 現在時刻 @@ -238,40 +237,40 @@ class ObjectTracker : public rclcpp::Node // 物体が検出されたら追従を行う if (look_object) { // 物体検出位置を取得 - auto object_position = object_point_msg_->point; + const auto object_position = object_point_msg_->point; // 追従動作のための首角度を計算 if (std::abs(object_position.x) > THRESH_X) { - yaw_angle_ -= object_position.x * OPERATION_GAIN_X; + target_angles_[0] -= object_position.x * OPERATION_GAIN_X; } if (std::abs(object_position.y) > THRESH_Y) { - pitch_angle_ -= object_position.y * OPERATION_GAIN_Y; + target_angles_[1] -= object_position.y * OPERATION_GAIN_Y; } } else { // ゆっくりと初期角度へ戻る - auto diff_yaw_angle = INITIAL_YAW_ANGLE - yaw_angle_; + auto diff_yaw_angle = INITIAL_YAW_ANGLE - target_angles_[0]; if (std::abs(diff_yaw_angle) > RESET_ANGLE_VEL) { - yaw_angle_ += std::copysign(RESET_ANGLE_VEL, diff_yaw_angle); + target_angles_[0] += std::copysign(RESET_ANGLE_VEL, diff_yaw_angle); } else { - yaw_angle_ = INITIAL_YAW_ANGLE; + target_angles_[0] = INITIAL_YAW_ANGLE; } - auto diff_pitch_angle = INITIAL_PITCH_ANGLE - pitch_angle_; + auto diff_pitch_angle = INITIAL_PITCH_ANGLE - target_angles_[1]; if (std::abs(diff_pitch_angle) > RESET_ANGLE_VEL) { - pitch_angle_ += std::copysign(RESET_ANGLE_VEL, diff_pitch_angle); + target_angles_[1] += std::copysign(RESET_ANGLE_VEL, diff_pitch_angle); } else { - pitch_angle_ = INITIAL_PITCH_ANGLE; + target_angles_[1] = INITIAL_PITCH_ANGLE; } } // 目標首角度を制限角度内に収める - yaw_angle_ = std::clamp(yaw_angle_, MIN_YAW_ANGLE, MAX_YAW_ANGLE); - pitch_angle_ = std::clamp(pitch_angle_, MIN_PITCH_ANGLE, MAX_PITCH_ANGLE); + 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(yaw_angle_); - target_angles_msg.data.push_back(pitch_angle_); + target_angles_msg.data.push_back(target_angles_[0]); + target_angles_msg.data.push_back(target_angles_[1]); angles_publisher_->publish(target_angles_msg); } }; From 806a4ca6da20142fe5789d4189577a45df710829 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Mon, 11 Dec 2023 16:19:26 +0900 Subject: [PATCH 11/55] =?UTF-8?q?component=20node=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_control/package.xml | 1 + sciurus17_examples/CMakeLists.txt | 56 ++- .../include/composition/color_detection.hpp | 57 +++ .../include/composition/neck_jt_control.hpp | 50 +++ .../include/composition/object_tracker.hpp | 61 ++++ .../launch/composition.launch.py | 30 ++ sciurus17_examples/src/color_detection.cpp | 130 +++++++ .../src/head_camera_tracking.cpp | 341 ------------------ sciurus17_examples/src/neck_jt_control.cpp | 74 ++++ sciurus17_examples/src/object_tracker.cpp | 159 ++++++++ 10 files changed, 617 insertions(+), 342 deletions(-) create mode 100644 sciurus17_examples/include/composition/color_detection.hpp create mode 100644 sciurus17_examples/include/composition/neck_jt_control.hpp create mode 100644 sciurus17_examples/include/composition/object_tracker.hpp create mode 100644 sciurus17_examples/launch/composition.launch.py create mode 100644 sciurus17_examples/src/color_detection.cpp delete mode 100644 sciurus17_examples/src/head_camera_tracking.cpp create mode 100644 sciurus17_examples/src/neck_jt_control.cpp create mode 100644 sciurus17_examples/src/object_tracker.cpp diff --git a/sciurus17_control/package.xml b/sciurus17_control/package.xml index dbc8310..b1195ff 100644 --- a/sciurus17_control/package.xml +++ b/sciurus17_control/package.xml @@ -18,6 +18,7 @@ hardware_interface pluginlib rclcpp + rclcpp_components ros2_controllers rt_manipulators_cpp diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index 1271147..6574a6a 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/sciurus17_examples/CMakeLists.txt @@ -15,6 +15,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +add_compile_options(-g) + # find dependencies find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) @@ -29,6 +31,59 @@ find_package(pcl_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) + +include_directories(include) + +add_library(color_detection SHARED + src/color_detection.cpp) +target_compile_definitions(color_detection + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(color_detection + rclcpp_components + rclcpp_action + cv_bridge + geometry_msgs + image_geometry + OpenCV + rclcpp + control_msgs) +rclcpp_components_register_nodes(color_detection "sciurus17_examples::ColorDetection") +set(node_plgins "${node_plugins}sciurus17_examples::ColorDetection;$\n") + +add_library(neck_jt_control SHARED + src/neck_jt_control.cpp) +target_compile_definitions(neck_jt_control + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(neck_jt_control + rclcpp_components + rclcpp_action + cv_bridge + geometry_msgs + image_geometry + OpenCV + rclcpp + control_msgs) +rclcpp_components_register_nodes(neck_jt_control + "sciurus17_examples::NeckJtControl") +set(node_plgins "${node_plugins}sciurus17_examples::NeckJtControl;$\n") + +add_library(object_tracker SHARED + src/object_tracker.cpp) +target_compile_definitions(object_tracker + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(object_tracker + rclcpp_components + rclcpp_action + cv_bridge + geometry_msgs + image_geometry + OpenCV + rclcpp + control_msgs) +rclcpp_components_register_nodes(object_tracker + "sciurus17_examples::ObjectTracker") +set(node_plgins "${node_plugins}sciurus17_examples::ObjectTracker;$\n") set(LIBRARY_NAME "pose_presets") add_library(${LIBRARY_NAME} @@ -75,7 +130,6 @@ set(executable_list waist_control pick_and_place_right_arm_waist pick_and_place_left_arm - head_camera_tracking ) foreach(loop_var IN LISTS executable_list) add_executable(${loop_var} src/${loop_var}.cpp) diff --git a/sciurus17_examples/include/composition/color_detection.hpp b/sciurus17_examples/include/composition/color_detection.hpp new file mode 100644 index 0000000..6bd17a4 --- /dev/null +++ b/sciurus17_examples/include/composition/color_detection.hpp @@ -0,0 +1,57 @@ +// 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 + +#ifndef COMPOSITION__COLOR_DETECTION_HPP_ +#define COMPOSITION__COLOR_DETECTION_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "opencv2/opencv.hpp" +using std::placeholders::_1; +using namespace std::chrono_literals; + +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_thresholded_publisher_; + rclcpp::Publisher::SharedPtr object_point_publisher_; + rclcpp::TimerBase::SharedPtr timer_; + cv::Point2d pixel_err_; + bool has_object_point_ = false; + // -1.0 ~ 1.0に正規化された検出位置 + cv::Point2d normalized_object_point_; + + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg); +}; + +} // namespace sciurus17_examples + +#endif // COMPOSITION__COLOR_DETECTION_HPP_ \ No newline at end of file diff --git a/sciurus17_examples/include/composition/neck_jt_control.hpp b/sciurus17_examples/include/composition/neck_jt_control.hpp new file mode 100644 index 0000000..5cf265c --- /dev/null +++ b/sciurus17_examples/include/composition/neck_jt_control.hpp @@ -0,0 +1,50 @@ +// 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 + +#ifndef COMPOSITION__NECK_JT_CONTROL_HPP_ +#define COMPOSITION__NECK_JT_CONTROL_HPP_ + +#include +#include +#include +#include +#include "angles/angles.h" + +#include "control_msgs/action/follow_joint_trajectory.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" +using std::placeholders::_1; +using namespace std::chrono_literals; + +namespace sciurus17_examples +{ + +class NeckJtControl : public rclcpp::Node +{ +public: + explicit NeckJtControl(const rclcpp::NodeOptions & options); + +private: + rclcpp_action::Client::SharedPtr client_ptr_; + rclcpp::Subscription::SharedPtr angles_subscription_; + void angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg); +}; + +} // namespace sciurus17_examples + +#endif // COMPOSITION__NECK_JT_CONTROL_HPP_ \ No newline at end of file diff --git a/sciurus17_examples/include/composition/object_tracker.hpp b/sciurus17_examples/include/composition/object_tracker.hpp new file mode 100644 index 0000000..eededc7 --- /dev/null +++ b/sciurus17_examples/include/composition/object_tracker.hpp @@ -0,0 +1,61 @@ +// 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 + +#ifndef COMPOSITION__OBJECT_TRACKER_HPP_ +#define COMPOSITION__OBJECT_TRACKER_HPP_ + +#include +#include +#include +#include +#include "angles/angles.h" + +#include "control_msgs/msg/joint_trajectory_controller_state.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" +#include "rclcpp/rclcpp.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 +{ + +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 // COMPOSITION__OBJECT_TRACKER_HPP_ \ No newline at end of file diff --git a/sciurus17_examples/launch/composition.launch.py b/sciurus17_examples/launch/composition.launch.py new file mode 100644 index 0000000..9d5cf6a --- /dev/null +++ b/sciurus17_examples/launch/composition.launch.py @@ -0,0 +1,30 @@ +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + """Generate launch description with multiple components.""" + container = ComposableNodeContainer( + name='my_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='sciurus17_examples', + plugin='sciurus17_examples::ColorDetection', + name='color_detection'), + ComposableNode( + package='sciurus17_examples', + plugin='sciurus17_examples::ObjectTracker', + name='object_tracker'), + ComposableNode( + package='sciurus17_examples', + plugin='sciurus17_examples::NeckJtControl', + name='neck_jt_control'), + ], + output='screen', + ) + + return launch.LaunchDescription([container]) \ No newline at end of file diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp new file mode 100644 index 0000000..e4e6e1d --- /dev/null +++ b/sciurus17_examples/src/color_detection.cpp @@ -0,0 +1,130 @@ +// 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 + +#include "composition/color_detection.hpp" + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.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; +using namespace std::chrono_literals; + +namespace sciurus17_examples +{ + +ColorDetection::ColorDetection(const rclcpp::NodeOptions & options) +: Node("color_detection", options) +{ + image_subscription_ = this->create_subscription( + "/head_camera/color/image_raw", 10, std::bind(&ColorDetection::image_callback, this, _1)); + + image_thresholded_publisher_ = + this->create_publisher("image_thresholded", 10); + + object_point_publisher_ = + this->create_publisher("object_detected", 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; + + // ウェブカメラの画像を受け取る + auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); + + // 画像をRGBからHSVに変換 + cv::cvtColor(cv_img->image, cv_img->image, cv::COLOR_RGB2HSV); + + // 画像処理用の変数を用意 + cv::Mat img_thresholded; + + // 画像の二値化 + cv::inRange( + cv_img->image, + 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))); + + // 閾値による二値化画像を配信 + sensor_msgs::msg::Image::SharedPtr img_thresholded_msg = + cv_bridge::CvImage(msg->header, "mono8", img_thresholded).toImageMsg(); + image_thresholded_publisher_->publish(*img_thresholded_msg); + + // 画像の検出領域におけるモーメントを計算 + cv::Moments moment = moments(img_thresholded); + double d_m01 = moment.m01; + double d_m10 = moment.m10; + double d_area = moment.m00; + + // 検出領域のピクセル数が100000より大きい場合 + has_object_point_ = d_area > 100000; + + if (has_object_point_) { + // 画像座標系における物体検出位置（2D） + cv::Point2d object_point; + object_point.x = d_m10 / d_area; + object_point.y = d_m01 / d_area; + + RCLCPP_INFO_STREAM(this->get_logger(), "Detect at" << object_point << "."); + + // 画像の中心を原点とした検出位置に変換 + 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; + + if (msg->width != 0 && msg->height) { + 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); + } +} + +} // namespace sciurus17_examples + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::ColorDetection) \ No newline at end of file diff --git a/sciurus17_examples/src/head_camera_tracking.cpp b/sciurus17_examples/src/head_camera_tracking.cpp deleted file mode 100644 index 0534bd9..0000000 --- a/sciurus17_examples/src/head_camera_tracking.cpp +++ /dev/null @@ -1,341 +0,0 @@ -// 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 - -#include -#include -#include -#include -#include "rclcpp_action/rclcpp_action.hpp" -#include "angles/angles.h" - -#include "std_msgs/msg/float64_multi_array.hpp" -#include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/point_stamped.hpp" -#include "sensor_msgs/msg/camera_info.hpp" -#include "sensor_msgs/msg/image.hpp" -#include "control_msgs/msg/joint_trajectory_controller_state.hpp" -#include "opencv2/opencv.hpp" -#include "cv_bridge/cv_bridge.h" -#include "control_msgs/action/follow_joint_trajectory.hpp" -using std::placeholders::_1; -using namespace std::chrono_literals; - -class ColorDetection : public rclcpp::Node -{ -public: - - ColorDetection() - : Node("color_detection") - { - image_subscription_ = this->create_subscription( - "/head_camera/color/image_raw", 10, std::bind(&ColorDetection::image_callback, this, _1)); - - image_thresholded_publisher_ = - this->create_publisher("image_thresholded", 10); - - object_point_publisher_ = - this->create_publisher("object_detected", 10); - } - -private: - rclcpp::Subscription::SharedPtr image_subscription_; - rclcpp::Publisher::SharedPtr image_thresholded_publisher_; - rclcpp::Publisher::SharedPtr object_point_publisher_; - rclcpp::TimerBase::SharedPtr timer_; - cv::Point2d pixel_err_; - bool has_object_point_ = false; - // -1.0 ~ 1.0に正規化された検出位置 - cv::Point2d normalized_object_point_; - - void 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; - - // ウェブカメラの画像を受け取る - auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); - - // 画像をRGBからHSVに変換 - cv::cvtColor(cv_img->image, cv_img->image, cv::COLOR_RGB2HSV); - - // 画像処理用の変数を用意 - cv::Mat img_thresholded; - - // 画像の二値化 - cv::inRange( - cv_img->image, - 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))); - - // 閾値による二値化画像を配信 - sensor_msgs::msg::Image::SharedPtr img_thresholded_msg = - cv_bridge::CvImage(msg->header, "mono8", img_thresholded).toImageMsg(); - image_thresholded_publisher_->publish(*img_thresholded_msg); - - // 画像の検出領域におけるモーメントを計算 - cv::Moments moment = moments(img_thresholded); - double d_m01 = moment.m01; - double d_m10 = moment.m10; - double d_area = moment.m00; - - // 検出領域のピクセル数が100000より大きい場合 - has_object_point_ = d_area > 100000; - - if (has_object_point_) { - // 画像座標系における物体検出位置（2D） - cv::Point2d object_point; - object_point.x = d_m10 / d_area; - object_point.y = d_m01 / d_area; - - RCLCPP_INFO_STREAM(this->get_logger(), "Detect at" << object_point << "."); - - // 画像の中心を原点とした検出位置に変換 - 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; - - if (msg->width != 0 && msg->height) { - 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); - } - } -}; - -class ObjectTracker : public rclcpp::Node -{ -public: - - ObjectTracker() - : Node("object_tracker") - { - timer_ = this->create_wall_timer( - 20ms, std::bind(&ObjectTracker::tracking, this)); - - state_subscription_ = this->create_subscription( - "/neck_controller/controller_state", 10, std::bind(&ObjectTracker::state_callback, this, _1)); - - object_point_subscription_ = this->create_subscription( - "/object_detected", 10, std::bind(&ObjectTracker::point_callback, this, _1)); - - angles_publisher_ = - this->create_publisher("/target_angles", 10); - } - -private: - rclcpp::TimerBase::SharedPtr timer_; - cv::Point2d pixel_err_; - // -1.0 ~ 1.0に正規化された検出位置 - cv::Point2d normalized_object_point_; - 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) - { - current_angles_msg_ = msg; - } - - void point_callback(const geometry_msgs::msg::PointStamped::SharedPtr msg) - { - object_point_msg_ = msg; - } - - void tracking() - { - // 追従を開始する物体位置の閾値 - const double THRESH_X = 0.03; - const double THRESH_Y= 0.03; - - // 首角度の初期値 - const double INITIAL_YAW_ANGLE = 0; - const double INITIAL_PITCH_ANGLE = 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(-85); - - // 首角度初期化時の制御角度 - const double RESET_ANGLE_VEL = angles::from_degrees(0.3); - - // 物体が検出されなくなってから初期角度に戻り始めるまでの時間 - const std::chrono::nanoseconds DETECTION_TIMEOUT = 3s; - - // 首角度制御量 - // 値が大きいほど追従速度が速くなる - const double OPERATION_GAIN_X = 0.05; - const double OPERATION_GAIN_Y = 0.05; - - // 追従動作開始フラグ - bool look_object = false; - - // 現在の首角度を取得 - if (!current_angles_msg_) { - RCLCPP_INFO_STREAM(this->get_logger(), "Wating controller state."); - return; - } - if (current_angles_msg_->feedback.positions.size() != 2) { - return; - } - const auto current_angles = current_angles_msg_->feedback.positions; - if (target_angles_.empty()) { - target_angles_ = current_angles; - } - - // 現在時刻 - 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) { - // 物体検出位置を取得 - const auto object_position = object_point_msg_->point; - - // 追従動作のための首角度を計算 - if (std::abs(object_position.x) > THRESH_X) { - target_angles_[0] -= object_position.x * OPERATION_GAIN_X; - } - if (std::abs(object_position.y) > THRESH_Y) { - target_angles_[1] -= object_position.y * OPERATION_GAIN_Y; - } - } else { - // ゆっくりと初期角度へ戻る - auto diff_yaw_angle = INITIAL_YAW_ANGLE - target_angles_[0]; - if (std::abs(diff_yaw_angle) > RESET_ANGLE_VEL) { - target_angles_[0] += std::copysign(RESET_ANGLE_VEL, diff_yaw_angle); - } else { - target_angles_[0] = INITIAL_YAW_ANGLE; - } - - auto diff_pitch_angle = INITIAL_PITCH_ANGLE - target_angles_[1]; - if (std::abs(diff_pitch_angle) > RESET_ANGLE_VEL) { - target_angles_[1] += std::copysign(RESET_ANGLE_VEL, diff_pitch_angle); - } else { - target_angles_[1] = INITIAL_PITCH_ANGLE; - } - } - - // 目標首角度を制限角度内に収める - 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); - } -}; - -class NeckControl : public rclcpp::Node -{ -public: - - NeckControl() - : Node("neck_control") - { - this->client_ptr_ = rclcpp_action::create_client( - this, - "neck_controller/follow_joint_trajectory"); - - angles_subscription_ = this->create_subscription( - "/target_angles", 10, std::bind(&NeckControl::angles_callback, this, _1)); - } - -private: - rclcpp_action::Client::SharedPtr client_ptr_; - rclcpp::Subscription::SharedPtr angles_subscription_; - control_msgs::msg::JointTrajectoryControllerState::SharedPtr neck_state_; - - void angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) { - using namespace std::placeholders; - auto yaw_angle = msg->data[0]; - auto pitch_angle = msg->data[1]; - const double TIME_FROM_START = 0.001; - - if(!this->client_ptr_->wait_for_action_server(5s)) { - RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); - rclcpp::shutdown(); - } - - auto goal_msg = control_msgs::action::FollowJointTrajectory::Goal(); - goal_msg.trajectory.joint_names.push_back("neck_yaw_joint"); - goal_msg.trajectory.joint_names.push_back("neck_pitch_joint"); - - trajectory_msgs::msg::JointTrajectoryPoint trajectory_point_msg; - trajectory_point_msg.positions.push_back(yaw_angle); - trajectory_point_msg.positions.push_back(pitch_angle); - trajectory_point_msg.time_from_start = rclcpp::Duration::from_seconds(TIME_FROM_START); - goal_msg.trajectory.points.push_back(trajectory_point_msg); - - this->client_ptr_->async_send_goal(goal_msg); - } -}; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - rclcpp::WallRate loop_rate(20ms); - node_options.automatically_declare_parameters_from_overrides(true); - - rclcpp::executors::MultiThreadedExecutor exec; - auto color_detection_node = std::make_shared(); - auto neck_control_node = std::make_shared(); - auto objeckt_tracker_node = std::make_shared(); - exec.add_node(color_detection_node); - exec.add_node(neck_control_node); - exec.add_node(objeckt_tracker_node); - exec.spin(); - - rclcpp::shutdown(); - return 0; -} diff --git a/sciurus17_examples/src/neck_jt_control.cpp b/sciurus17_examples/src/neck_jt_control.cpp new file mode 100644 index 0000000..3794161 --- /dev/null +++ b/sciurus17_examples/src/neck_jt_control.cpp @@ -0,0 +1,74 @@ +// 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 + +#include "composition/neck_jt_control.hpp" + +#include +#include +#include +#include +#include "angles/angles.h" + +#include "control_msgs/action/follow_joint_trajectory.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.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) +{ + this->client_ptr_ = rclcpp_action::create_client( + this, + "neck_controller/follow_joint_trajectory"); + + angles_subscription_ = this->create_subscription( + "/target_angles", 10, std::bind(&NeckJtControl::angles_callback, this, _1)); +} + +void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) { + using namespace std::placeholders; + auto yaw_angle = msg->data[0]; + auto pitch_angle = msg->data[1]; + const double TIME_FROM_START = 0.001; + + if(!this->client_ptr_->wait_for_action_server(5s)) { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + } + + auto goal_msg = control_msgs::action::FollowJointTrajectory::Goal(); + goal_msg.trajectory.joint_names.push_back("neck_yaw_joint"); + goal_msg.trajectory.joint_names.push_back("neck_pitch_joint"); + + trajectory_msgs::msg::JointTrajectoryPoint trajectory_point_msg; + trajectory_point_msg.positions.push_back(yaw_angle); + trajectory_point_msg.positions.push_back(pitch_angle); + trajectory_point_msg.time_from_start = rclcpp::Duration::from_seconds(TIME_FROM_START); + goal_msg.trajectory.points.push_back(trajectory_point_msg); + + this->client_ptr_->async_send_goal(goal_msg); +} + +} // namespace sciurus17_examples + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::NeckJtControl) \ No newline at end of file diff --git a/sciurus17_examples/src/object_tracker.cpp b/sciurus17_examples/src/object_tracker.cpp new file mode 100644 index 0000000..a40cfc6 --- /dev/null +++ b/sciurus17_examples/src/object_tracker.cpp @@ -0,0 +1,159 @@ +// 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 + +#include "composition/object_tracker.hpp" + +#include +#include +#include +#include +#include "angles/angles.h" + +#include "control_msgs/msg/joint_trajectory_controller_state.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" +#include "rclcpp/rclcpp.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( + 20ms, std::bind(&ObjectTracker::tracking, this)); + + state_subscription_ = this->create_subscription( + "/neck_controller/controller_state", 10, std::bind(&ObjectTracker::state_callback, this, _1)); + + object_point_subscription_ = this->create_subscription( + "/object_detected", 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 THRESH_X = 0.03; + const double THRESH_Y= 0.03; + + // 首角度の初期値 + const double INITIAL_YAW_ANGLE = 0; + const double INITIAL_PITCH_ANGLE = 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(-85); + + // 首角度初期化時の制御角度 + const double RESET_ANGLE_VEL = angles::from_degrees(0.3); + + // 物体が検出されなくなってから初期角度に戻り始めるまでの時間 + const std::chrono::nanoseconds DETECTION_TIMEOUT = 3s; + + // 首角度制御量 + // 値が大きいほど追従速度が速くなる + const double OPERATION_GAIN_X = 0.05; + const double OPERATION_GAIN_Y = 0.05; + + // 追従動作開始フラグ + bool look_object = false; + + // 現在の首角度を取得 + if (!current_angles_msg_) { + RCLCPP_INFO_STREAM(this->get_logger(), "Wating controller state."); + return; + } + if (current_angles_msg_->feedback.positions.size() != 2) { + return; + } + const auto current_angles = current_angles_msg_->feedback.positions; + if (target_angles_.empty()) { + target_angles_ = current_angles; + } + + // 現在時刻 + 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) { + // 物体検出位置を取得 + const auto object_position = object_point_msg_->point; + + // 追従動作のための首角度を計算 + if (std::abs(object_position.x) > THRESH_X) { + target_angles_[0] -= object_position.x * OPERATION_GAIN_X; + } + if (std::abs(object_position.y) > THRESH_Y) { + target_angles_[1] -= object_position.y * OPERATION_GAIN_Y; + } + } else { + // ゆっくりと初期角度へ戻る + auto diff_yaw_angle = INITIAL_YAW_ANGLE - target_angles_[0]; + if (std::abs(diff_yaw_angle) > RESET_ANGLE_VEL) { + target_angles_[0] += std::copysign(RESET_ANGLE_VEL, diff_yaw_angle); + } else { + target_angles_[0] = INITIAL_YAW_ANGLE; + } + + auto diff_pitch_angle = INITIAL_PITCH_ANGLE - target_angles_[1]; + if (std::abs(diff_pitch_angle) > RESET_ANGLE_VEL) { + target_angles_[1] += std::copysign(RESET_ANGLE_VEL, diff_pitch_angle); + } else { + target_angles_[1] = INITIAL_PITCH_ANGLE; + } + } + + // 目標首角度を制限角度内に収める + 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) \ No newline at end of file From 9964ce575784f2745e76479a08aa28e2e5687fe1 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Mon, 11 Dec 2023 17:30:07 +0900 Subject: [PATCH 12/55] =?UTF-8?q?install=E8=A8=98=E8=BF=B0=E8=BF=BD?= =?UTF-8?q?=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/CMakeLists.txt | 77 ++++++++++++++----------------- 1 file changed, 34 insertions(+), 43 deletions(-) diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index 6574a6a..0ac0a3a 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/sciurus17_examples/CMakeLists.txt @@ -35,56 +35,47 @@ find_package(rclcpp_components REQUIRED) include_directories(include) -add_library(color_detection SHARED - src/color_detection.cpp) -target_compile_definitions(color_detection - PRIVATE "COMPOSITION_BUILDING_DLL") -ament_target_dependencies(color_detection - rclcpp_components - rclcpp_action - cv_bridge - geometry_msgs - image_geometry - OpenCV - rclcpp - control_msgs) +set(components_list + color_detection + neck_jt_control + object_tracker +) +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") set(node_plgins "${node_plugins}sciurus17_examples::ColorDetection;$\n") -add_library(neck_jt_control SHARED - src/neck_jt_control.cpp) -target_compile_definitions(neck_jt_control - PRIVATE "COMPOSITION_BUILDING_DLL") -ament_target_dependencies(neck_jt_control - rclcpp_components - rclcpp_action - cv_bridge - geometry_msgs - image_geometry - OpenCV - rclcpp - control_msgs) -rclcpp_components_register_nodes(neck_jt_control - "sciurus17_examples::NeckJtControl") +rclcpp_components_register_nodes(neck_jt_control "sciurus17_examples::NeckJtControl") set(node_plgins "${node_plugins}sciurus17_examples::NeckJtControl;$\n") -add_library(object_tracker SHARED - src/object_tracker.cpp) -target_compile_definitions(object_tracker - PRIVATE "COMPOSITION_BUILDING_DLL") -ament_target_dependencies(object_tracker - rclcpp_components - rclcpp_action - cv_bridge - geometry_msgs - image_geometry - OpenCV - rclcpp - control_msgs) -rclcpp_components_register_nodes(object_tracker - "sciurus17_examples::ObjectTracker") +rclcpp_components_register_nodes(object_tracker "sciurus17_examples::ObjectTracker") set(node_plgins "${node_plugins}sciurus17_examples::ObjectTracker;$\n") +install(TARGETS + color_detection + object_tracker + neck_jt_control + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + set(LIBRARY_NAME "pose_presets") add_library(${LIBRARY_NAME} SHARED From 1e9ecfb80db4533393e873c85b5332e7432c6eb1 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Mon, 11 Dec 2023 18:49:52 +0900 Subject: [PATCH 13/55] rename --- .../launch/{composition.launch.py => object_tracking.launch.py} | 1 - 1 file changed, 1 deletion(-) rename sciurus17_examples/launch/{composition.launch.py => object_tracking.launch.py} (94%) diff --git a/sciurus17_examples/launch/composition.launch.py b/sciurus17_examples/launch/object_tracking.launch.py similarity index 94% rename from sciurus17_examples/launch/composition.launch.py rename to sciurus17_examples/launch/object_tracking.launch.py index 9d5cf6a..ae5bb83 100644 --- a/sciurus17_examples/launch/composition.launch.py +++ b/sciurus17_examples/launch/object_tracking.launch.py @@ -4,7 +4,6 @@ def generate_launch_description(): - """Generate launch description with multiple components.""" container = ComposableNodeContainer( name='my_container', namespace='', From 428eaecf0b4986421e83900b99785c51ba64ede9 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Mon, 11 Dec 2023 20:23:49 +0900 Subject: [PATCH 14/55] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AA=E5=A4=89?= =?UTF-8?q?=E6=95=B0=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/composition/color_detection.hpp | 1 - sciurus17_examples/src/color_detection.cpp | 10 ++++------ 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/sciurus17_examples/include/composition/color_detection.hpp b/sciurus17_examples/include/composition/color_detection.hpp index 6bd17a4..5f7d42e 100644 --- a/sciurus17_examples/include/composition/color_detection.hpp +++ b/sciurus17_examples/include/composition/color_detection.hpp @@ -45,7 +45,6 @@ class ColorDetection : public rclcpp::Node rclcpp::Publisher::SharedPtr object_point_publisher_; rclcpp::TimerBase::SharedPtr timer_; cv::Point2d pixel_err_; - bool has_object_point_ = false; // -1.0 ~ 1.0に正規化された検出位置 cv::Point2d normalized_object_point_; diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index e4e6e1d..cc85cbe 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -44,7 +44,7 @@ ColorDetection::ColorDetection(const rclcpp::NodeOptions & options) this->create_publisher("image_thresholded", 10); object_point_publisher_ = - this->create_publisher("object_detected", 10); + this->create_publisher("target_position", 10); } void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) @@ -97,9 +97,7 @@ void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg double d_area = moment.m00; // 検出領域のピクセル数が100000より大きい場合 - has_object_point_ = d_area > 100000; - - if (has_object_point_) { + if (d_area > 100000) { // 画像座標系における物体検出位置（2D） cv::Point2d object_point; object_point.x = d_m10 / d_area; @@ -113,8 +111,8 @@ void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg translated_object_point.y = object_point.y - msg->height / 2.0; if (msg->width != 0 && msg->height) { - normalized_object_point_.x = translated_object_point.x / msg->width / 2.0; - normalized_object_point_.y = translated_object_point.y / msg->height / 2.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; From adf17c98d66cdfaae7758167cf3d4a3f93459d6f Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Mon, 11 Dec 2023 20:25:54 +0900 Subject: [PATCH 15/55] =?UTF-8?q?=E9=85=8D=E5=88=97=E3=82=B5=E3=82=A4?= =?UTF-8?q?=E3=82=BA=E3=81=AE=E3=83=81=E3=82=A7=E3=83=83=E3=82=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/neck_jt_control.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/sciurus17_examples/src/neck_jt_control.cpp b/sciurus17_examples/src/neck_jt_control.cpp index 3794161..86b3f49 100644 --- a/sciurus17_examples/src/neck_jt_control.cpp +++ b/sciurus17_examples/src/neck_jt_control.cpp @@ -46,9 +46,13 @@ NeckJtControl::NeckJtControl(const rclcpp::NodeOptions & options) void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) { using namespace std::placeholders; + const double TIME_FROM_START = 0.001; + + if (msg->data.size() != 2) { + return; + } auto yaw_angle = msg->data[0]; auto pitch_angle = msg->data[1]; - const double TIME_FROM_START = 0.001; if(!this->client_ptr_->wait_for_action_server(5s)) { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); From f496cab8baf811598506133eaabbd284ad85f23b Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Tue, 12 Dec 2023 12:27:26 +0900 Subject: [PATCH 16/55] =?UTF-8?q?yaw=E3=80=81pitch=E8=BB=B8=E3=81=AE?= =?UTF-8?q?=E5=87=A6=E7=90=86=E3=82=92for=E6=96=87=E3=81=AB=E7=BD=AE?= =?UTF-8?q?=E3=81=8D=E6=8F=9B=E3=81=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/object_tracker.cpp | 53 +++++++++++------------ 1 file changed, 25 insertions(+), 28 deletions(-) diff --git a/sciurus17_examples/src/object_tracker.cpp b/sciurus17_examples/src/object_tracker.cpp index a40cfc6..3a8ef18 100644 --- a/sciurus17_examples/src/object_tracker.cpp +++ b/sciurus17_examples/src/object_tracker.cpp @@ -44,7 +44,7 @@ ObjectTracker::ObjectTracker(const rclcpp::NodeOptions & options) "/neck_controller/controller_state", 10, std::bind(&ObjectTracker::state_callback, this, _1)); object_point_subscription_ = this->create_subscription( - "/object_detected", 10, std::bind(&ObjectTracker::point_callback, this, _1)); + "/target_position", 10, std::bind(&ObjectTracker::point_callback, this, _1)); angles_publisher_ = this->create_publisher("/target_angles", 10); @@ -63,29 +63,26 @@ void ObjectTracker::point_callback(const geometry_msgs::msg::PointStamped::Share void ObjectTracker::tracking() { // 追従を開始する物体位置の閾値 - const double THRESH_X = 0.03; - const double THRESH_Y= 0.03; + const double POSITION_THRESH = 0.05; // 首角度の初期値 - const double INITIAL_YAW_ANGLE = 0; - const double INITIAL_PITCH_ANGLE = 0; + 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(-85); + const double MIN_PITCH_ANGLE = angles::from_degrees(-75); // 首角度初期化時の制御角度 - const double RESET_ANGLE_VEL = angles::from_degrees(0.3); + const double MAX_ANGULAR_VEL = angles::from_degrees(0.5); // 物体が検出されなくなってから初期角度に戻り始めるまでの時間 - const std::chrono::nanoseconds DETECTION_TIMEOUT = 3s; + const std::chrono::nanoseconds DETECTION_TIMEOUT = 1s; // 首角度制御量 // 値が大きいほど追従速度が速くなる - const double OPERATION_GAIN_X = 0.05; - const double OPERATION_GAIN_Y = 0.05; + const double OPERATION_GAIN = 0.05; // 追従動作開始フラグ bool look_object = false; @@ -116,29 +113,29 @@ void ObjectTracker::tracking() // 物体が検出されたら追従を行う if (look_object) { // 物体検出位置を取得 - const auto object_position = object_point_msg_->point; + 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}; // 追従動作のための首角度を計算 - if (std::abs(object_position.x) > THRESH_X) { - target_angles_[0] -= object_position.x * OPERATION_GAIN_X; - } - if (std::abs(object_position.y) > THRESH_Y) { - target_angles_[1] -= object_position.y * OPERATION_GAIN_Y; + 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_VEL, MAX_ANGULAR_VEL); + target_angles_[i] -= diff_angles[i]; + } } } else { // ゆっくりと初期角度へ戻る - auto diff_yaw_angle = INITIAL_YAW_ANGLE - target_angles_[0]; - if (std::abs(diff_yaw_angle) > RESET_ANGLE_VEL) { - target_angles_[0] += std::copysign(RESET_ANGLE_VEL, diff_yaw_angle); - } else { - target_angles_[0] = INITIAL_YAW_ANGLE; - } - - auto diff_pitch_angle = INITIAL_PITCH_ANGLE - target_angles_[1]; - if (std::abs(diff_pitch_angle) > RESET_ANGLE_VEL) { - target_angles_[1] += std::copysign(RESET_ANGLE_VEL, diff_pitch_angle); - } else { - target_angles_[1] = INITIAL_PITCH_ANGLE; + 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]) > MAX_ANGULAR_VEL) { + target_angles_[i] += std::copysign(MAX_ANGULAR_VEL, diff_angles[i]); + } else { + target_angles_[i] = INITIAL_ANGLES[i]; + } } } From 31013e279dac679b0015fc8d4a6a8a59c2097b6f Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Tue, 12 Dec 2023 14:43:34 +0900 Subject: [PATCH 17/55] =?UTF-8?q?action=20server=E3=81=AE=E7=A2=BA?= =?UTF-8?q?=E8=AA=8D=E3=82=92=E3=82=B3=E3=83=B3=E3=82=B9=E3=83=88=E3=83=A9?= =?UTF-8?q?=E3=82=AF=E3=82=BF=E5=86=85=E3=81=A7=E5=AE=9F=E8=A1=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/neck_jt_control.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/sciurus17_examples/src/neck_jt_control.cpp b/sciurus17_examples/src/neck_jt_control.cpp index 86b3f49..d381a81 100644 --- a/sciurus17_examples/src/neck_jt_control.cpp +++ b/sciurus17_examples/src/neck_jt_control.cpp @@ -36,17 +36,22 @@ 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)); + this->client_ptr_ = rclcpp_action::create_client( this, "neck_controller/follow_joint_trajectory"); - angles_subscription_ = this->create_subscription( - "/target_angles", 10, std::bind(&NeckJtControl::angles_callback, this, _1)); + if(!this->client_ptr_->wait_for_action_server(5s)) { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + } } void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) { using namespace std::placeholders; - const double TIME_FROM_START = 0.001; + const double TIME_FROM_START = 1.0e-9; if (msg->data.size() != 2) { return; @@ -54,11 +59,6 @@ void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::Shar auto yaw_angle = msg->data[0]; auto pitch_angle = msg->data[1]; - if(!this->client_ptr_->wait_for_action_server(5s)) { - RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); - rclcpp::shutdown(); - } - auto goal_msg = control_msgs::action::FollowJointTrajectory::Goal(); goal_msg.trajectory.joint_names.push_back("neck_yaw_joint"); goal_msg.trajectory.joint_names.push_back("neck_pitch_joint"); From 73d8de7c174626d1a4c73432a7e8d16b04a02b8a Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Tue, 12 Dec 2023 15:29:01 +0900 Subject: [PATCH 18/55] =?UTF-8?q?=E3=83=97=E3=83=AD=E3=82=BB=E3=82=B9?= =?UTF-8?q?=E5=86=85=E9=80=9A=E4=BF=A1=E3=82=92=E6=9C=89=E5=8A=B9=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/launch/object_tracking.launch.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/sciurus17_examples/launch/object_tracking.launch.py b/sciurus17_examples/launch/object_tracking.launch.py index ae5bb83..89eca31 100644 --- a/sciurus17_examples/launch/object_tracking.launch.py +++ b/sciurus17_examples/launch/object_tracking.launch.py @@ -13,15 +13,21 @@ def generate_launch_description(): ComposableNode( package='sciurus17_examples', plugin='sciurus17_examples::ColorDetection', - name='color_detection'), + name='color_detection', + extra_arguments=[{'use_intra_process_comms': True}] + ), ComposableNode( package='sciurus17_examples', plugin='sciurus17_examples::ObjectTracker', - name='object_tracker'), + name='object_tracker', + extra_arguments=[{'use_intra_process_comms': True}] + ), ComposableNode( package='sciurus17_examples', plugin='sciurus17_examples::NeckJtControl', - name='neck_jt_control'), + name='neck_jt_control', + extra_arguments=[{'use_intra_process_comms': True}] + ), ], output='screen', ) From 6addaee3e5333295bccd1559c39022891ca2b225 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Tue, 12 Dec 2023 18:40:11 +0900 Subject: [PATCH 19/55] =?UTF-8?q?action=E5=8B=95=E4=BD=9C=E3=81=8C?= =?UTF-8?q?=E5=AE=8C=E4=BA=86=E3=81=97=E3=81=9F=E3=81=8B=E7=A2=BA=E8=AA=8D?= =?UTF-8?q?=E3=81=99=E3=82=8B=E5=87=A6=E7=90=86=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/composition/neck_jt_control.hpp | 3 ++ sciurus17_examples/src/neck_jt_control.cpp | 28 +++++++++++++++++-- 2 files changed, 29 insertions(+), 2 deletions(-) diff --git a/sciurus17_examples/include/composition/neck_jt_control.hpp b/sciurus17_examples/include/composition/neck_jt_control.hpp index 5cf265c..1dbd61d 100644 --- a/sciurus17_examples/include/composition/neck_jt_control.hpp +++ b/sciurus17_examples/include/composition/neck_jt_control.hpp @@ -42,7 +42,10 @@ class NeckJtControl : public rclcpp::Node private: rclcpp_action::Client::SharedPtr client_ptr_; rclcpp::Subscription::SharedPtr angles_subscription_; + bool has_result_ = true; + void angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg); + void result_callback(const rclcpp_action::ClientGoalHandle::WrappedResult & result); }; } // namespace sciurus17_examples diff --git a/sciurus17_examples/src/neck_jt_control.cpp b/sciurus17_examples/src/neck_jt_control.cpp index d381a81..0eb342e 100644 --- a/sciurus17_examples/src/neck_jt_control.cpp +++ b/sciurus17_examples/src/neck_jt_control.cpp @@ -50,9 +50,12 @@ NeckJtControl::NeckJtControl(const rclcpp::NodeOptions & options) } void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) { - using namespace std::placeholders; const double TIME_FROM_START = 1.0e-9; + if (!has_result_) { + return; + } + if (msg->data.size() != 2) { return; } @@ -69,7 +72,28 @@ void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::Shar trajectory_point_msg.time_from_start = rclcpp::Duration::from_seconds(TIME_FROM_START); goal_msg.trajectory.points.push_back(trajectory_point_msg); - this->client_ptr_->async_send_goal(goal_msg); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback =std::bind(&NeckJtControl::result_callback, this, _1); + + this->client_ptr_->async_send_goal(goal_msg, send_goal_options); + has_result_ = false; +} + +void NeckJtControl::result_callback(const rclcpp_action::ClientGoalHandle::WrappedResult & result) { + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; + } + has_result_ = true; } } // namespace sciurus17_examples From 285b2977b4dbcb4c14859ba74d7705bdc6149a31 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Tue, 12 Dec 2023 19:02:02 +0900 Subject: [PATCH 20/55] =?UTF-8?q?=E3=82=B3=E3=83=A1=E3=83=B3=E3=83=88?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/composition/color_detection.hpp | 14 -------------- sciurus17_examples/src/color_detection.cpp | 13 +++++-------- 2 files changed, 5 insertions(+), 22 deletions(-) diff --git a/sciurus17_examples/include/composition/color_detection.hpp b/sciurus17_examples/include/composition/color_detection.hpp index 5f7d42e..ee726ae 100644 --- a/sciurus17_examples/include/composition/color_detection.hpp +++ b/sciurus17_examples/include/composition/color_detection.hpp @@ -12,24 +12,13 @@ // 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 - #ifndef COMPOSITION__COLOR_DETECTION_HPP_ #define COMPOSITION__COLOR_DETECTION_HPP_ -#include -#include -#include -#include - #include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/point_stamped.hpp" #include "sensor_msgs/msg/image.hpp" #include "opencv2/opencv.hpp" -using std::placeholders::_1; -using namespace std::chrono_literals; namespace sciurus17_examples { @@ -44,9 +33,6 @@ class ColorDetection : public rclcpp::Node rclcpp::Publisher::SharedPtr image_thresholded_publisher_; rclcpp::Publisher::SharedPtr object_point_publisher_; rclcpp::TimerBase::SharedPtr timer_; - cv::Point2d pixel_err_; - // -1.0 ~ 1.0に正規化された検出位置 - cv::Point2d normalized_object_point_; void image_callback(const sensor_msgs::msg::Image::SharedPtr msg); }; diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index cc85cbe..e0f3f36 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -17,19 +17,12 @@ #include "composition/color_detection.hpp" -#include -#include -#include -#include - #include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.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; -using namespace std::chrono_literals; namespace sciurus17_examples { @@ -110,10 +103,14 @@ void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg translated_object_point.x = object_point.x - msg->width / 2.0; translated_object_point.y = object_point.y - msg->height / 2.0; - if (msg->width != 0 && msg->height) { + // 検出位置を-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; From 9509062867a0587c7cfab8bfe3bf4fb1a2cb73dd Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Tue, 12 Dec 2023 19:12:55 +0900 Subject: [PATCH 21/55] =?UTF-8?q?=E3=82=B3=E3=83=A1=E3=83=B3=E3=83=88?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/composition/neck_jt_control.hpp | 13 +------------ sciurus17_examples/src/neck_jt_control.cpp | 19 +++++++++---------- 2 files changed, 10 insertions(+), 22 deletions(-) diff --git a/sciurus17_examples/include/composition/neck_jt_control.hpp b/sciurus17_examples/include/composition/neck_jt_control.hpp index 1dbd61d..c20d8ac 100644 --- a/sciurus17_examples/include/composition/neck_jt_control.hpp +++ b/sciurus17_examples/include/composition/neck_jt_control.hpp @@ -12,24 +12,13 @@ // 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 - #ifndef COMPOSITION__NECK_JT_CONTROL_HPP_ #define COMPOSITION__NECK_JT_CONTROL_HPP_ -#include -#include -#include -#include -#include "angles/angles.h" - -#include "control_msgs/action/follow_joint_trajectory.hpp" #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 { diff --git a/sciurus17_examples/src/neck_jt_control.cpp b/sciurus17_examples/src/neck_jt_control.cpp index 0eb342e..55fb2e1 100644 --- a/sciurus17_examples/src/neck_jt_control.cpp +++ b/sciurus17_examples/src/neck_jt_control.cpp @@ -12,20 +12,11 @@ // 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 - #include "composition/neck_jt_control.hpp" -#include -#include -#include -#include -#include "angles/angles.h" - -#include "control_msgs/action/follow_joint_trajectory.hpp" #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; @@ -50,22 +41,27 @@ NeckJtControl::NeckJtControl(const rclcpp::NodeOptions & options) } void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) { + // 動作時間 const double TIME_FROM_START = 1.0e-9; + // 動作完了していない場合はgoalを配信しない if (!has_result_) { return; } + // 角度指令値取得 if (msg->data.size() != 2) { return; } auto yaw_angle = msg->data[0]; auto pitch_angle = msg->data[1]; + // joint名設定 auto goal_msg = control_msgs::action::FollowJointTrajectory::Goal(); goal_msg.trajectory.joint_names.push_back("neck_yaw_joint"); goal_msg.trajectory.joint_names.push_back("neck_pitch_joint"); + // 角度指令値設定 trajectory_msgs::msg::JointTrajectoryPoint trajectory_point_msg; trajectory_point_msg.positions.push_back(yaw_angle); trajectory_point_msg.positions.push_back(pitch_angle); @@ -75,6 +71,7 @@ void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::Shar auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); send_goal_options.result_callback =std::bind(&NeckJtControl::result_callback, this, _1); + // 角度指令値配信 this->client_ptr_->async_send_goal(goal_msg, send_goal_options); has_result_ = false; } @@ -93,6 +90,8 @@ void NeckJtControl::result_callback(const rclcpp_action::ClientGoalHandleget_logger(), "Unknown result code"); return; } + + // 動作完了フラグをtrueにする has_result_ = true; } From 73c5a3b28fefa238215f5dcd7560bda916475363 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Tue, 12 Dec 2023 19:36:12 +0900 Subject: [PATCH 22/55] =?UTF-8?q?=E8=A7=92=E5=BA=A6=E5=88=B6=E9=99=90?= =?UTF-8?q?=E3=81=AE=E5=AE=9A=E7=BE=A9=E7=AE=87=E6=89=80=E3=82=92=E5=A4=89?= =?UTF-8?q?=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/composition/object_tracker.hpp | 15 +-------- sciurus17_examples/src/neck_jt_control.cpp | 12 +++++++ sciurus17_examples/src/object_tracker.cpp | 32 +++++-------------- 3 files changed, 21 insertions(+), 38 deletions(-) diff --git a/sciurus17_examples/include/composition/object_tracker.hpp b/sciurus17_examples/include/composition/object_tracker.hpp index eededc7..78ca04e 100644 --- a/sciurus17_examples/include/composition/object_tracker.hpp +++ b/sciurus17_examples/include/composition/object_tracker.hpp @@ -12,25 +12,14 @@ // 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 - #ifndef COMPOSITION__OBJECT_TRACKER_HPP_ #define COMPOSITION__OBJECT_TRACKER_HPP_ -#include -#include -#include -#include -#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 "rclcpp/rclcpp.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 { @@ -50,9 +39,7 @@ class ObjectTracker : public rclcpp::Node 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(); }; diff --git a/sciurus17_examples/src/neck_jt_control.cpp b/sciurus17_examples/src/neck_jt_control.cpp index 55fb2e1..8aa9d03 100644 --- a/sciurus17_examples/src/neck_jt_control.cpp +++ b/sciurus17_examples/src/neck_jt_control.cpp @@ -14,6 +14,8 @@ #include "composition/neck_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" @@ -43,6 +45,12 @@ NeckJtControl::NeckJtControl(const rclcpp::NodeOptions & options) void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) { // 動作時間 const double TIME_FROM_START = 1.0e-9; + // 首可動範囲 + 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); + // 動作完了していない場合はgoalを配信しない if (!has_result_) { @@ -56,6 +64,10 @@ void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::Shar 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名設定 auto goal_msg = control_msgs::action::FollowJointTrajectory::Goal(); goal_msg.trajectory.joint_names.push_back("neck_yaw_joint"); diff --git a/sciurus17_examples/src/object_tracker.cpp b/sciurus17_examples/src/object_tracker.cpp index 3a8ef18..b832102 100644 --- a/sciurus17_examples/src/object_tracker.cpp +++ b/sciurus17_examples/src/object_tracker.cpp @@ -12,20 +12,13 @@ // 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 - #include "composition/object_tracker.hpp" -#include -#include -#include -#include #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 "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/image.hpp" #include "std_msgs/msg/float64_multi_array.hpp" using std::placeholders::_1; @@ -65,15 +58,9 @@ void ObjectTracker::tracking() // 追従を開始する物体位置の閾値 const double POSITION_THRESH = 0.05; - // 首角度の初期値 + // 角度指令値の初期値 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 double MAX_ANGULAR_VEL = angles::from_degrees(0.5); @@ -84,7 +71,7 @@ void ObjectTracker::tracking() // 値が大きいほど追従速度が速くなる const double OPERATION_GAIN = 0.05; - // 追従動作開始フラグ + // 追従フラグ bool look_object = false; // 現在の首角度を取得 @@ -100,11 +87,11 @@ void ObjectTracker::tracking() target_angles_ = current_angles; } - // 現在時刻 + // 現在時刻取得 auto now = this->get_clock()->now().nanoseconds(); if (object_point_msg_) { - // 物体を検出したとき追従動作開始フラグをtrueにする + // タイムアウト時間以内に物体を検出したとき追従フラグを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(); @@ -118,7 +105,7 @@ void ObjectTracker::tracking() 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; @@ -129,6 +116,7 @@ void ObjectTracker::tracking() } 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]) > MAX_ANGULAR_VEL) { @@ -139,11 +127,7 @@ void ObjectTracker::tracking() } } - // 目標首角度を制限角度内に収める - 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]); From c879424d6d699b5c5b6a140b45268eb328e1d0f4 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Tue, 12 Dec 2023 19:54:57 +0900 Subject: [PATCH 23/55] =?UTF-8?q?=E3=82=B3=E3=83=A1=E3=83=B3=E3=83=88?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/object_tracker.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/sciurus17_examples/src/object_tracker.cpp b/sciurus17_examples/src/object_tracker.cpp index b832102..7057658 100644 --- a/sciurus17_examples/src/object_tracker.cpp +++ b/sciurus17_examples/src/object_tracker.cpp @@ -56,20 +56,20 @@ void ObjectTracker::point_callback(const geometry_msgs::msg::PointStamped::Share void ObjectTracker::tracking() { // 追従を開始する物体位置の閾値 - const double POSITION_THRESH = 0.05; + const double POSITION_THRESH = 0.1; // 角度指令値の初期値 const std::vector INITIAL_ANGLES = {0, 0}; - // 首角度初期化時の制御角度 - const double MAX_ANGULAR_VEL = angles::from_degrees(0.5); + // 最大角度変化量 + const double MAX_ANGULAR_DIFF = angles::from_degrees(0.5); // 物体が検出されなくなってから初期角度に戻り始めるまでの時間 const std::chrono::nanoseconds DETECTION_TIMEOUT = 1s; // 首角度制御量 // 値が大きいほど追従速度が速くなる - const double OPERATION_GAIN = 0.05; + const double OPERATION_GAIN = 0.02; // 追従フラグ bool look_object = false; @@ -109,7 +109,7 @@ void ObjectTracker::tracking() 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_VEL, MAX_ANGULAR_VEL); + diff_angles[i] = std::clamp(diff_angles[i], -MAX_ANGULAR_DIFF, MAX_ANGULAR_DIFF); target_angles_[i] -= diff_angles[i]; } } @@ -119,8 +119,8 @@ void ObjectTracker::tracking() for (int i = 0; i < 2; i++) { diff_angles[i] = INITIAL_ANGLES[i] - target_angles_[i]; - if (std::abs(diff_angles[i]) > MAX_ANGULAR_VEL) { - target_angles_[i] += std::copysign(MAX_ANGULAR_VEL, diff_angles[i]); + if (std::abs(diff_angles[i]) > MAX_ANGULAR_DIFF) { + target_angles_[i] += std::copysign(MAX_ANGULAR_DIFF, diff_angles[i]); } else { target_angles_[i] = INITIAL_ANGLES[i]; } From 63965e2106a0d228068adabbc84ed7943a9670b3 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Tue, 12 Dec 2023 20:02:02 +0900 Subject: [PATCH 24/55] =?UTF-8?q?=E3=82=B9=E3=82=BF=E3=82=A4=E3=83=AB?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/object_tracking.launch.py | 50 +++++++++---------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/sciurus17_examples/launch/object_tracking.launch.py b/sciurus17_examples/launch/object_tracking.launch.py index 89eca31..8a06249 100644 --- a/sciurus17_examples/launch/object_tracking.launch.py +++ b/sciurus17_examples/launch/object_tracking.launch.py @@ -5,31 +5,31 @@ def generate_launch_description(): container = ComposableNodeContainer( - name='my_container', - namespace='', - package='rclcpp_components', - executable='component_container', - composable_node_descriptions=[ - ComposableNode( - package='sciurus17_examples', - plugin='sciurus17_examples::ColorDetection', - name='color_detection', - extra_arguments=[{'use_intra_process_comms': True}] - ), - ComposableNode( - package='sciurus17_examples', - plugin='sciurus17_examples::ObjectTracker', - name='object_tracker', - extra_arguments=[{'use_intra_process_comms': True}] - ), - ComposableNode( - package='sciurus17_examples', - plugin='sciurus17_examples::NeckJtControl', - name='neck_jt_control', - extra_arguments=[{'use_intra_process_comms': True}] - ), - ], - output='screen', + name='tracking_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='sciurus17_examples', + plugin='sciurus17_examples::ColorDetection', + name='color_detection', + extra_arguments=[{'use_intra_process_comms': True}] + ), + ComposableNode( + package='sciurus17_examples', + plugin='sciurus17_examples::ObjectTracker', + name='object_tracker', + extra_arguments=[{'use_intra_process_comms': True}] + ), + ComposableNode( + package='sciurus17_examples', + plugin='sciurus17_examples::NeckJtControl', + name='neck_jt_control', + extra_arguments=[{'use_intra_process_comms': True}] + ), + ], + output='screen', ) return launch.LaunchDescription([container]) \ No newline at end of file From e9dd3f22f7fb4c4648998f33ac6b73baac0fdff0 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Tue, 12 Dec 2023 20:02:19 +0900 Subject: [PATCH 25/55] =?UTF-8?q?=E3=83=91=E3=83=A9=E3=83=A1=E3=83=BC?= =?UTF-8?q?=E3=82=BF=E8=AA=BF=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/object_tracker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples/src/object_tracker.cpp b/sciurus17_examples/src/object_tracker.cpp index 7057658..e606bed 100644 --- a/sciurus17_examples/src/object_tracker.cpp +++ b/sciurus17_examples/src/object_tracker.cpp @@ -62,7 +62,7 @@ void ObjectTracker::tracking() const std::vector INITIAL_ANGLES = {0, 0}; // 最大角度変化量 - const double MAX_ANGULAR_DIFF = angles::from_degrees(0.5); + const double MAX_ANGULAR_DIFF = angles::from_degrees(1.0); // 物体が検出されなくなってから初期角度に戻り始めるまでの時間 const std::chrono::nanoseconds DETECTION_TIMEOUT = 1s; From b4fac9177e61fccb327e502ed39257f9b96d98b9 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Tue, 12 Dec 2023 20:28:37 +0900 Subject: [PATCH 26/55] =?UTF-8?q?=E3=82=B9=E3=82=BF=E3=82=A4=E3=83=AB?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/composition/color_detection.hpp | 2 +- .../include/composition/neck_jt_control.hpp | 6 ++++-- .../include/composition/object_tracker.hpp | 7 +++++-- .../launch/object_tracking.launch.py | 16 +++++++++++++++- sciurus17_examples/src/color_detection.cpp | 2 +- sciurus17_examples/src/neck_jt_control.cpp | 18 ++++++++++++------ sciurus17_examples/src/object_tracker.cpp | 8 +++++--- 7 files changed, 43 insertions(+), 16 deletions(-) diff --git a/sciurus17_examples/include/composition/color_detection.hpp b/sciurus17_examples/include/composition/color_detection.hpp index ee726ae..14ac5d2 100644 --- a/sciurus17_examples/include/composition/color_detection.hpp +++ b/sciurus17_examples/include/composition/color_detection.hpp @@ -39,4 +39,4 @@ class ColorDetection : public rclcpp::Node } // namespace sciurus17_examples -#endif // COMPOSITION__COLOR_DETECTION_HPP_ \ No newline at end of file +#endif // COMPOSITION__COLOR_DETECTION_HPP_ diff --git a/sciurus17_examples/include/composition/neck_jt_control.hpp b/sciurus17_examples/include/composition/neck_jt_control.hpp index c20d8ac..72b6137 100644 --- a/sciurus17_examples/include/composition/neck_jt_control.hpp +++ b/sciurus17_examples/include/composition/neck_jt_control.hpp @@ -20,6 +20,8 @@ #include "control_msgs/action/follow_joint_trajectory.hpp" #include "std_msgs/msg/float64_multi_array.hpp" +using GoalHandleJt = rclcpp_action::ClientGoalHandle; + namespace sciurus17_examples { @@ -34,9 +36,9 @@ class NeckJtControl : public rclcpp::Node bool has_result_ = true; void angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg); - void result_callback(const rclcpp_action::ClientGoalHandle::WrappedResult & result); + void result_callback(const GoalHandleJt::WrappedResult & result); }; } // namespace sciurus17_examples -#endif // COMPOSITION__NECK_JT_CONTROL_HPP_ \ No newline at end of file +#endif // COMPOSITION__NECK_JT_CONTROL_HPP_ diff --git a/sciurus17_examples/include/composition/object_tracker.hpp b/sciurus17_examples/include/composition/object_tracker.hpp index 78ca04e..84c718d 100644 --- a/sciurus17_examples/include/composition/object_tracker.hpp +++ b/sciurus17_examples/include/composition/object_tracker.hpp @@ -15,6 +15,8 @@ #ifndef COMPOSITION__OBJECT_TRACKER_HPP_ #define COMPOSITION__OBJECT_TRACKER_HPP_ +#include + #include "rclcpp/rclcpp.hpp" #include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "geometry_msgs/msg/point_stamped.hpp" @@ -31,7 +33,8 @@ class ObjectTracker : public rclcpp::Node private: rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Subscription::SharedPtr state_subscription_; + rclcpp::Subscription::SharedPtr + state_subscription_; rclcpp::Subscription::SharedPtr object_point_subscription_; rclcpp::Publisher::SharedPtr angles_publisher_; control_msgs::msg::JointTrajectoryControllerState::SharedPtr current_angles_msg_; @@ -45,4 +48,4 @@ class ObjectTracker : public rclcpp::Node } // namespace sciurus17_examples -#endif // COMPOSITION__OBJECT_TRACKER_HPP_ \ No newline at end of file +#endif // COMPOSITION__OBJECT_TRACKER_HPP_ diff --git a/sciurus17_examples/launch/object_tracking.launch.py b/sciurus17_examples/launch/object_tracking.launch.py index 8a06249..daeead1 100644 --- a/sciurus17_examples/launch/object_tracking.launch.py +++ b/sciurus17_examples/launch/object_tracking.launch.py @@ -1,3 +1,17 @@ +# 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. + import launch from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode @@ -32,4 +46,4 @@ def generate_launch_description(): output='screen', ) - return launch.LaunchDescription([container]) \ No newline at end of file + return launch.LaunchDescription([container]) diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index e0f3f36..aa704ec 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -122,4 +122,4 @@ void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg } // namespace sciurus17_examples #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::ColorDetection) \ No newline at end of file +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 index 8aa9d03..8e57605 100644 --- a/sciurus17_examples/src/neck_jt_control.cpp +++ b/sciurus17_examples/src/neck_jt_control.cpp @@ -20,8 +20,10 @@ #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; +using GoalHandleJt = rclcpp_action::ClientGoalHandle; namespace sciurus17_examples { @@ -36,13 +38,14 @@ NeckJtControl::NeckJtControl(const rclcpp::NodeOptions & options) this, "neck_controller/follow_joint_trajectory"); - if(!this->client_ptr_->wait_for_action_server(5s)) { + if (!this->client_ptr_->wait_for_action_server(5s)) { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); rclcpp::shutdown(); } } -void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) { +void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) +{ // 動作時間 const double TIME_FROM_START = 1.0e-9; // 首可動範囲 @@ -80,15 +83,18 @@ void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::Shar trajectory_point_msg.time_from_start = rclcpp::Duration::from_seconds(TIME_FROM_START); goal_msg.trajectory.points.push_back(trajectory_point_msg); - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.result_callback =std::bind(&NeckJtControl::result_callback, this, _1); + auto send_goal_options = + rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = std::bind(&NeckJtControl::result_callback, this, _1); // 角度指令値配信 this->client_ptr_->async_send_goal(goal_msg, send_goal_options); has_result_ = false; } -void NeckJtControl::result_callback(const rclcpp_action::ClientGoalHandle::WrappedResult & result) { +void NeckJtControl::result_callback( + const GoalHandleJt::WrappedResult & result) +{ switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: break; @@ -110,4 +116,4 @@ void NeckJtControl::result_callback(const rclcpp_action::ClientGoalHandlecreate_wall_timer( 20ms, std::bind(&ObjectTracker::tracking, this)); - state_subscription_ = this->create_subscription( + state_subscription_ = + this->create_subscription( "/neck_controller/controller_state", 10, std::bind(&ObjectTracker::state_callback, this, _1)); object_point_subscription_ = this->create_subscription( @@ -43,7 +44,8 @@ ObjectTracker::ObjectTracker(const rclcpp::NodeOptions & options) this->create_publisher("/target_angles", 10); } -void ObjectTracker::state_callback(const control_msgs::msg::JointTrajectoryControllerState::SharedPtr msg) +void ObjectTracker::state_callback( + const control_msgs::msg::JointTrajectoryControllerState::SharedPtr msg) { current_angles_msg_ = msg; } @@ -137,4 +139,4 @@ void ObjectTracker::tracking() } // namespace sciurus17_examples #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::ObjectTracker) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::ObjectTracker) From 5d97674d093bbdde4344628cc6b3569bf879f30b Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Wed, 13 Dec 2023 14:47:54 +0900 Subject: [PATCH 27/55] =?UTF-8?q?use=5Fsim=5Ftime=E3=82=AA=E3=83=97?= =?UTF-8?q?=E3=82=B7=E3=83=A7=E3=83=B3=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/object_tracking.launch.py | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/sciurus17_examples/launch/object_tracking.launch.py b/sciurus17_examples/launch/object_tracking.launch.py index daeead1..367962c 100644 --- a/sciurus17_examples/launch/object_tracking.launch.py +++ b/sciurus17_examples/launch/object_tracking.launch.py @@ -12,12 +12,20 @@ # See the License for the specific language governing permissions and # limitations under the License. -import launch +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='', @@ -46,4 +54,8 @@ def generate_launch_description(): output='screen', ) - return launch.LaunchDescription([container]) + return LaunchDescription([ + declare_use_sim_time, + SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), + container + ]) From b5776d4dfafa1485a088137d4f6f064ebcc1c58f Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Wed, 13 Dec 2023 14:55:37 +0900 Subject: [PATCH 28/55] =?UTF-8?q?=E3=82=B3=E3=83=A1=E3=83=B3=E3=83=88?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/object_tracker.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sciurus17_examples/src/object_tracker.cpp b/sciurus17_examples/src/object_tracker.cpp index 99add03..bf623c9 100644 --- a/sciurus17_examples/src/object_tracker.cpp +++ b/sciurus17_examples/src/object_tracker.cpp @@ -69,14 +69,14 @@ void ObjectTracker::tracking() // 物体が検出されなくなってから初期角度に戻り始めるまでの時間 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(), "Wating controller state."); return; From 8cabc07c9ba03ac2367d23ecfadf29e2ffffc55e Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Wed, 13 Dec 2023 14:57:23 +0900 Subject: [PATCH 29/55] =?UTF-8?q?README=E6=9B=B4=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/README.md | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index 46c8136..b983fef 100644 --- a/sciurus17_examples/README.md +++ b/sciurus17_examples/README.md @@ -17,6 +17,7 @@ - [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) + - [object\_tracking](#object_tracking) ## 準備（実機を使う場合） @@ -89,6 +90,7 @@ 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) +- [object\_tracking](#object_tracking) 実行できるサンプルの一覧は、`example.launch.py`にオプション`-s`を付けて実行することで表示できます。 @@ -165,3 +167,20 @@ ros2 launch sciurus17_examples example.launch.py example:='pick_and_place_left_a [back to example list](#examples) --- + +### object_tracking + +カメラ映像を用いてオレンジ色の物体を追従するコード例です。 + +Gazeboで実行する場合は動作環境によってうまく追従しない場合があります。 +カメラ解像度やサンプルコード内の追従速度ゲインを調整してください。 + +次のコマンドを実行します。 + +```sh +ros2 launch sciurus17_examples object_tracking.launch.py +``` + +[back to example list](#examples) + +--- From 424eaaedcc3652086adf170eb9e50110554284bb Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Wed, 13 Dec 2023 15:02:16 +0900 Subject: [PATCH 30/55] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AA=E8=A8=98?= =?UTF-8?q?=E8=BF=B0=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index 0ac0a3a..6caff20 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/sciurus17_examples/CMakeLists.txt @@ -15,8 +15,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -add_compile_options(-g) - # find dependencies find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) From 77c9f542eadbdb6b637821c8e4d87eb8b0e9d943 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Wed, 13 Dec 2023 15:38:31 +0900 Subject: [PATCH 31/55] =?UTF-8?q?=E4=BE=9D=E5=AD=98=E3=83=91=E3=83=83?= =?UTF-8?q?=E3=82=B1=E3=83=BC=E3=82=B8=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_control/package.xml | 1 - sciurus17_examples/package.xml | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_control/package.xml b/sciurus17_control/package.xml index b1195ff..dbc8310 100644 --- a/sciurus17_control/package.xml +++ b/sciurus17_control/package.xml @@ -18,7 +18,6 @@ hardware_interface pluginlib rclcpp - rclcpp_components ros2_controllers rt_manipulators_cpp diff --git a/sciurus17_examples/package.xml b/sciurus17_examples/package.xml index 1c8557e..6339169 100644 --- a/sciurus17_examples/package.xml +++ b/sciurus17_examples/package.xml @@ -26,6 +26,7 @@ moveit_ros_planning_interface pcl_ros rclcpp + rclcpp_components realsense2_camera tf2_geometry_msgs From 8a8a7708d129e3bd3560c4ef58983bf437ed60ab Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Wed, 13 Dec 2023 15:39:11 +0900 Subject: [PATCH 32/55] =?UTF-8?q?=E3=83=87=E3=82=A3=E3=83=AC=E3=82=AF?= =?UTF-8?q?=E3=83=88=E3=83=AA=E5=90=8D=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../{composition => sciurus17_examples}/color_detection.hpp | 0 .../{composition => sciurus17_examples}/neck_jt_control.hpp | 0 .../{composition => sciurus17_examples}/object_tracker.hpp | 0 sciurus17_examples/src/color_detection.cpp | 2 +- sciurus17_examples/src/neck_jt_control.cpp | 2 +- sciurus17_examples/src/object_tracker.cpp | 2 +- 6 files changed, 3 insertions(+), 3 deletions(-) rename sciurus17_examples/include/{composition => sciurus17_examples}/color_detection.hpp (100%) rename sciurus17_examples/include/{composition => sciurus17_examples}/neck_jt_control.hpp (100%) rename sciurus17_examples/include/{composition => sciurus17_examples}/object_tracker.hpp (100%) diff --git a/sciurus17_examples/include/composition/color_detection.hpp b/sciurus17_examples/include/sciurus17_examples/color_detection.hpp similarity index 100% rename from sciurus17_examples/include/composition/color_detection.hpp rename to sciurus17_examples/include/sciurus17_examples/color_detection.hpp diff --git a/sciurus17_examples/include/composition/neck_jt_control.hpp b/sciurus17_examples/include/sciurus17_examples/neck_jt_control.hpp similarity index 100% rename from sciurus17_examples/include/composition/neck_jt_control.hpp rename to sciurus17_examples/include/sciurus17_examples/neck_jt_control.hpp diff --git a/sciurus17_examples/include/composition/object_tracker.hpp b/sciurus17_examples/include/sciurus17_examples/object_tracker.hpp similarity index 100% rename from sciurus17_examples/include/composition/object_tracker.hpp rename to sciurus17_examples/include/sciurus17_examples/object_tracker.hpp diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index aa704ec..0095f15 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -15,7 +15,7 @@ // Reference: // https://www.opencv-srf.com/2010/09/object-detection-using-color-seperation.html -#include "composition/color_detection.hpp" +#include "sciurus17_examples/color_detection.hpp" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/point_stamped.hpp" diff --git a/sciurus17_examples/src/neck_jt_control.cpp b/sciurus17_examples/src/neck_jt_control.cpp index 8e57605..7a2e288 100644 --- a/sciurus17_examples/src/neck_jt_control.cpp +++ b/sciurus17_examples/src/neck_jt_control.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "composition/neck_jt_control.hpp" +#include "sciurus17_examples/neck_jt_control.hpp" #include "angles/angles.h" diff --git a/sciurus17_examples/src/object_tracker.cpp b/sciurus17_examples/src/object_tracker.cpp index bf623c9..aaa69f7 100644 --- a/sciurus17_examples/src/object_tracker.cpp +++ b/sciurus17_examples/src/object_tracker.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "composition/object_tracker.hpp" +#include "sciurus17_examples/object_tracker.hpp" #include "angles/angles.h" From 9f6afd9547e9fc3f06457b29f30647915ad4740b Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Wed, 13 Dec 2023 16:21:26 +0900 Subject: [PATCH 33/55] =?UTF-8?q?=E8=AA=A4=E5=AD=97=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/sciurus17_examples/color_detection.hpp | 6 +++--- .../include/sciurus17_examples/neck_jt_control.hpp | 6 +++--- .../include/sciurus17_examples/object_tracker.hpp | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/sciurus17_examples/include/sciurus17_examples/color_detection.hpp b/sciurus17_examples/include/sciurus17_examples/color_detection.hpp index 14ac5d2..abdc7af 100644 --- a/sciurus17_examples/include/sciurus17_examples/color_detection.hpp +++ b/sciurus17_examples/include/sciurus17_examples/color_detection.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMPOSITION__COLOR_DETECTION_HPP_ -#define COMPOSITION__COLOR_DETECTION_HPP_ +#ifndef SCIURUS17_EXAMPLES__COLOR_DETECTION_HPP_ +#define SCIURUS17_EXAMPLES__COLOR_DETECTION_HPP_ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/point_stamped.hpp" @@ -39,4 +39,4 @@ class ColorDetection : public rclcpp::Node } // namespace sciurus17_examples -#endif // COMPOSITION__COLOR_DETECTION_HPP_ +#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 index 72b6137..9b2083a 100644 --- a/sciurus17_examples/include/sciurus17_examples/neck_jt_control.hpp +++ b/sciurus17_examples/include/sciurus17_examples/neck_jt_control.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMPOSITION__NECK_JT_CONTROL_HPP_ -#define COMPOSITION__NECK_JT_CONTROL_HPP_ +#ifndef SCIURUS17_EXAMPLES__NECK_JT_CONTROL_HPP_ +#define SCIURUS17_EXAMPLES__NECK_JT_CONTROL_HPP_ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -41,4 +41,4 @@ class NeckJtControl : public rclcpp::Node } // namespace sciurus17_examples -#endif // COMPOSITION__NECK_JT_CONTROL_HPP_ +#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 index 84c718d..c3057ba 100644 --- a/sciurus17_examples/include/sciurus17_examples/object_tracker.hpp +++ b/sciurus17_examples/include/sciurus17_examples/object_tracker.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMPOSITION__OBJECT_TRACKER_HPP_ -#define COMPOSITION__OBJECT_TRACKER_HPP_ +#ifndef SCIURUS17_EXAMPLES__OBJECT_TRACKER_HPP_ +#define SCIURUS17_EXAMPLES__OBJECT_TRACKER_HPP_ #include @@ -48,4 +48,4 @@ class ObjectTracker : public rclcpp::Node } // namespace sciurus17_examples -#endif // COMPOSITION__OBJECT_TRACKER_HPP_ +#endif // SCIURUS17_EXAMPLES__OBJECT_TRACKER_HPP_ From 88f11890bca4dcf3b7ef0f9282bd578988794fc3 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Fri, 15 Dec 2023 15:41:22 +0900 Subject: [PATCH 34/55] =?UTF-8?q?joint=5Ftrajectory=E3=81=AE=E9=85=8D?= =?UTF-8?q?=E4=BF=A1=E3=82=92action=E3=81=8B=E3=82=89topic=E3=81=AB?= =?UTF-8?q?=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sciurus17_examples/neck_jt_control.hpp | 9 +-- sciurus17_examples/src/neck_jt_control.cpp | 57 +++---------------- sciurus17_examples/src/object_tracker.cpp | 4 +- 3 files changed, 12 insertions(+), 58 deletions(-) diff --git a/sciurus17_examples/include/sciurus17_examples/neck_jt_control.hpp b/sciurus17_examples/include/sciurus17_examples/neck_jt_control.hpp index 9b2083a..249362d 100644 --- a/sciurus17_examples/include/sciurus17_examples/neck_jt_control.hpp +++ b/sciurus17_examples/include/sciurus17_examples/neck_jt_control.hpp @@ -16,12 +16,9 @@ #define SCIURUS17_EXAMPLES__NECK_JT_CONTROL_HPP_ #include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "control_msgs/action/follow_joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" #include "std_msgs/msg/float64_multi_array.hpp" -using GoalHandleJt = rclcpp_action::ClientGoalHandle; - namespace sciurus17_examples { @@ -31,12 +28,10 @@ class NeckJtControl : public rclcpp::Node explicit NeckJtControl(const rclcpp::NodeOptions & options); private: - rclcpp_action::Client::SharedPtr client_ptr_; rclcpp::Subscription::SharedPtr angles_subscription_; - bool has_result_ = true; + rclcpp::Publisher::SharedPtr jt_publisher_; void angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg); - void result_callback(const GoalHandleJt::WrappedResult & result); }; } // namespace sciurus17_examples diff --git a/sciurus17_examples/src/neck_jt_control.cpp b/sciurus17_examples/src/neck_jt_control.cpp index 7a2e288..8df06b7 100644 --- a/sciurus17_examples/src/neck_jt_control.cpp +++ b/sciurus17_examples/src/neck_jt_control.cpp @@ -17,13 +17,11 @@ #include "angles/angles.h" #include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "control_msgs/action/follow_joint_trajectory.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; -using GoalHandleJt = rclcpp_action::ClientGoalHandle; namespace sciurus17_examples { @@ -34,14 +32,8 @@ NeckJtControl::NeckJtControl(const rclcpp::NodeOptions & options) angles_subscription_ = this->create_subscription( "/target_angles", 10, std::bind(&NeckJtControl::angles_callback, this, _1)); - this->client_ptr_ = rclcpp_action::create_client( - this, - "neck_controller/follow_joint_trajectory"); - - if (!this->client_ptr_->wait_for_action_server(5s)) { - RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); - rclcpp::shutdown(); - } + jt_publisher_ = this->create_publisher( + "neck_controller/joint_trajectory", 10); } void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) @@ -54,12 +46,6 @@ void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::Shar const double MAX_PITCH_ANGLE = angles::from_degrees(50); const double MIN_PITCH_ANGLE = angles::from_degrees(-75); - - // 動作完了していない場合はgoalを配信しない - if (!has_result_) { - return; - } - // 角度指令値取得 if (msg->data.size() != 2) { return; @@ -72,45 +58,18 @@ void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::Shar pitch_angle = std::clamp(pitch_angle, MIN_PITCH_ANGLE, MAX_PITCH_ANGLE); // joint名設定 - auto goal_msg = control_msgs::action::FollowJointTrajectory::Goal(); - goal_msg.trajectory.joint_names.push_back("neck_yaw_joint"); - goal_msg.trajectory.joint_names.push_back("neck_pitch_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 trajectory_point_msg; trajectory_point_msg.positions.push_back(yaw_angle); trajectory_point_msg.positions.push_back(pitch_angle); trajectory_point_msg.time_from_start = rclcpp::Duration::from_seconds(TIME_FROM_START); - goal_msg.trajectory.points.push_back(trajectory_point_msg); - - auto send_goal_options = - rclcpp_action::Client::SendGoalOptions(); - send_goal_options.result_callback = std::bind(&NeckJtControl::result_callback, this, _1); - - // 角度指令値配信 - this->client_ptr_->async_send_goal(goal_msg, send_goal_options); - has_result_ = false; -} - -void NeckJtControl::result_callback( - const GoalHandleJt::WrappedResult & result) -{ - switch (result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - return; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - return; - } + jt_msg.points.push_back(trajectory_point_msg); - // 動作完了フラグをtrueにする - has_result_ = true; + jt_publisher_->publish(jt_msg); } } // namespace sciurus17_examples diff --git a/sciurus17_examples/src/object_tracker.cpp b/sciurus17_examples/src/object_tracker.cpp index aaa69f7..4a704e4 100644 --- a/sciurus17_examples/src/object_tracker.cpp +++ b/sciurus17_examples/src/object_tracker.cpp @@ -31,7 +31,7 @@ ObjectTracker::ObjectTracker(const rclcpp::NodeOptions & options) : Node("object_tracker", options) { timer_ = this->create_wall_timer( - 20ms, std::bind(&ObjectTracker::tracking, this)); + 10ms, std::bind(&ObjectTracker::tracking, this)); state_subscription_ = this->create_subscription( @@ -64,7 +64,7 @@ void ObjectTracker::tracking() const std::vector INITIAL_ANGLES = {0, 0}; // 最大角度変化量 - const double MAX_ANGULAR_DIFF = angles::from_degrees(1.0); + const double MAX_ANGULAR_DIFF = angles::from_degrees(0.5); // 物体が検出されなくなってから初期角度に戻り始めるまでの時間 const std::chrono::nanoseconds DETECTION_TIMEOUT = 1s; From 6d2d154b7e03649a517fde18255b740eb328333c Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Fri, 15 Dec 2023 21:09:33 +0900 Subject: [PATCH 35/55] =?UTF-8?q?=E5=8B=95=E4=BD=9C=E5=91=A8=E6=9C=9F?= =?UTF-8?q?=E8=AA=BF=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/neck_jt_control.cpp | 4 ++-- sciurus17_examples/src/object_tracker.cpp | 11 +++++++---- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/sciurus17_examples/src/neck_jt_control.cpp b/sciurus17_examples/src/neck_jt_control.cpp index 8df06b7..de00f30 100644 --- a/sciurus17_examples/src/neck_jt_control.cpp +++ b/sciurus17_examples/src/neck_jt_control.cpp @@ -39,7 +39,7 @@ NeckJtControl::NeckJtControl(const rclcpp::NodeOptions & options) void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) { // 動作時間 - const double TIME_FROM_START = 1.0e-9; + const auto TIME_FROM_START = 1ms; // 首可動範囲 const double MAX_YAW_ANGLE = angles::from_degrees(120); const double MIN_YAW_ANGLE = angles::from_degrees(-120); @@ -66,7 +66,7 @@ void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::Shar trajectory_msgs::msg::JointTrajectoryPoint trajectory_point_msg; trajectory_point_msg.positions.push_back(yaw_angle); trajectory_point_msg.positions.push_back(pitch_angle); - trajectory_point_msg.time_from_start = rclcpp::Duration::from_seconds(TIME_FROM_START); + trajectory_point_msg.time_from_start = rclcpp::Duration(TIME_FROM_START); jt_msg.points.push_back(trajectory_point_msg); jt_publisher_->publish(jt_msg); diff --git a/sciurus17_examples/src/object_tracker.cpp b/sciurus17_examples/src/object_tracker.cpp index 4a704e4..1badca3 100644 --- a/sciurus17_examples/src/object_tracker.cpp +++ b/sciurus17_examples/src/object_tracker.cpp @@ -31,7 +31,7 @@ ObjectTracker::ObjectTracker(const rclcpp::NodeOptions & options) : Node("object_tracker", options) { timer_ = this->create_wall_timer( - 10ms, std::bind(&ObjectTracker::tracking, this)); + 30ms, std::bind(&ObjectTracker::tracking, this)); state_subscription_ = this->create_subscription( @@ -64,7 +64,10 @@ void ObjectTracker::tracking() const std::vector INITIAL_ANGLES = {0, 0}; // 最大角度変化量 - const double MAX_ANGULAR_DIFF = angles::from_degrees(0.5); + 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; @@ -121,8 +124,8 @@ void ObjectTracker::tracking() for (int i = 0; i < 2; i++) { diff_angles[i] = INITIAL_ANGLES[i] - target_angles_[i]; - if (std::abs(diff_angles[i]) > MAX_ANGULAR_DIFF) { - target_angles_[i] += std::copysign(MAX_ANGULAR_DIFF, diff_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]; } From 472f82eacfdcb8394a2b14493c01056b889739d4 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Mon, 18 Dec 2023 12:27:05 +0900 Subject: [PATCH 36/55] =?UTF-8?q?=E8=85=B0=E8=BF=BD=E5=BE=93=E5=8B=95?= =?UTF-8?q?=E4=BD=9C=E7=94=A8=E3=82=B3=E3=83=BC=E3=83=89=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sciurus17_examples/waist_jt_control.hpp | 44 +++++++ sciurus17_examples/src/waist_jt_control.cpp | 117 ++++++++++++++++++ 2 files changed, 161 insertions(+) create mode 100644 sciurus17_examples/include/sciurus17_examples/waist_jt_control.hpp create mode 100644 sciurus17_examples/src/waist_jt_control.cpp 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..0ff516c --- /dev/null +++ b/sciurus17_examples/include/sciurus17_examples/waist_jt_control.hpp @@ -0,0 +1,44 @@ +// 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 "control_msgs/action/follow_joint_trajectory.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" + +using GoalHandleJt = rclcpp_action::ClientGoalHandle; + +namespace sciurus17_examples +{ + +class WaistJtControl : public rclcpp::Node +{ +public: + explicit WaistJtControl(const rclcpp::NodeOptions & options); + +private: + rclcpp_action::Client::SharedPtr client_ptr_; + rclcpp::Subscription::SharedPtr angles_subscription_; + bool has_result_ = true; + + void angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg); + void result_callback(const GoalHandleJt::WrappedResult & result); +}; + +} // namespace sciurus17_examples + +#endif // SCIURUS17_EXAMPLES__WAIST_JT_CONTROL_HPP_ diff --git a/sciurus17_examples/src/waist_jt_control.cpp b/sciurus17_examples/src/waist_jt_control.cpp new file mode 100644 index 0000000..2dc5f7a --- /dev/null +++ b/sciurus17_examples/src/waist_jt_control.cpp @@ -0,0 +1,117 @@ +// 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; +using GoalHandleJt = rclcpp_action::ClientGoalHandle; + +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)); + + this->client_ptr_ = rclcpp_action::create_client( + this, + "waist_yaw_controller/follow_joint_trajectory"); + + if (!this->client_ptr_->wait_for_action_server(5s)) { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + } +} + +void WaistJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) +{ + // 動作時間 + const double TIME_FROM_START = 1.0e-9; + // 首可動範囲 + 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); + + + // 動作完了していない場合はgoalを配信しない + if (!has_result_) { + return; + } + + // 角度指令値取得 + 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名設定 + auto goal_msg = control_msgs::action::FollowJointTrajectory::Goal(); + goal_msg.trajectory.joint_names.push_back("waist_yaw_joint"); + + // 角度指令値設定 + trajectory_msgs::msg::JointTrajectoryPoint trajectory_point_msg; + trajectory_point_msg.positions.push_back(yaw_angle); + trajectory_point_msg.time_from_start = rclcpp::Duration::from_seconds(TIME_FROM_START); + goal_msg.trajectory.points.push_back(trajectory_point_msg); + + auto send_goal_options = + rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = std::bind(&WaistJtControl::result_callback, this, _1); + + // 角度指令値配信 + this->client_ptr_->async_send_goal(goal_msg, send_goal_options); + has_result_ = false; +} + +void WaistJtControl::result_callback( + const GoalHandleJt::WrappedResult & result) +{ + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; + } + + // 動作完了フラグをtrueにする + has_result_ = true; +} + +} // namespace sciurus17_examples + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::WaistJtControl) From d3367f324c6e24aa47ea777a3f66ae43832af47d Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Mon, 18 Dec 2023 14:21:13 +0900 Subject: [PATCH 37/55] =?UTF-8?q?=E3=83=95=E3=82=A1=E3=82=A4=E3=83=AB?= =?UTF-8?q?=E5=90=8D=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../{object_tracking.launch.py => head_camera_tracking.launch.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename sciurus17_examples/launch/{object_tracking.launch.py => head_camera_tracking.launch.py} (100%) diff --git a/sciurus17_examples/launch/object_tracking.launch.py b/sciurus17_examples/launch/head_camera_tracking.launch.py similarity index 100% rename from sciurus17_examples/launch/object_tracking.launch.py rename to sciurus17_examples/launch/head_camera_tracking.launch.py From d4c999fdb8aa84ddb4b23f0be9eeb90db4cb47bb Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Mon, 18 Dec 2023 18:04:03 +0900 Subject: [PATCH 38/55] =?UTF-8?q?waist=5Fjt=5Fcontroller=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/CMakeLists.txt | 7 +- .../sciurus17_examples/waist_jt_control.hpp | 8 +-- sciurus17_examples/src/waist_jt_control.cpp | 66 ++++--------------- 3 files changed, 19 insertions(+), 62 deletions(-) diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index 6caff20..68c3651 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/sciurus17_examples/CMakeLists.txt @@ -37,6 +37,7 @@ 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 @@ -64,10 +65,14 @@ set(node_plgins "${node_plugins}sciurus17_examples::NeckJtControl;$\n") +rclcpp_components_register_nodes(waist_jt_control "sciurus17_examples::WaistJtControl") +set(node_plgins "${node_plugins}sciurus17_examples::WaistJtControl;$\n") + install(TARGETS color_detection - object_tracker neck_jt_control + object_tracker + waist_jt_control ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/sciurus17_examples/include/sciurus17_examples/waist_jt_control.hpp b/sciurus17_examples/include/sciurus17_examples/waist_jt_control.hpp index 0ff516c..4a19957 100644 --- a/sciurus17_examples/include/sciurus17_examples/waist_jt_control.hpp +++ b/sciurus17_examples/include/sciurus17_examples/waist_jt_control.hpp @@ -17,11 +17,9 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" -#include "control_msgs/action/follow_joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" #include "std_msgs/msg/float64_multi_array.hpp" -using GoalHandleJt = rclcpp_action::ClientGoalHandle; - namespace sciurus17_examples { @@ -31,12 +29,10 @@ class WaistJtControl : public rclcpp::Node explicit WaistJtControl(const rclcpp::NodeOptions & options); private: - rclcpp_action::Client::SharedPtr client_ptr_; rclcpp::Subscription::SharedPtr angles_subscription_; - bool has_result_ = true; + rclcpp::Publisher::SharedPtr jt_publisher_; void angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg); - void result_callback(const GoalHandleJt::WrappedResult & result); }; } // namespace sciurus17_examples diff --git a/sciurus17_examples/src/waist_jt_control.cpp b/sciurus17_examples/src/waist_jt_control.cpp index 2dc5f7a..f99aaf2 100644 --- a/sciurus17_examples/src/waist_jt_control.cpp +++ b/sciurus17_examples/src/waist_jt_control.cpp @@ -23,7 +23,6 @@ using std::placeholders::_1; using namespace std::chrono_literals; -using GoalHandleJt = rclcpp_action::ClientGoalHandle; namespace sciurus17_examples { @@ -32,83 +31,40 @@ 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)); + "target_angles", 10, std::bind(&WaistJtControl::angles_callback, this, _1)); - this->client_ptr_ = rclcpp_action::create_client( - this, - "waist_yaw_controller/follow_joint_trajectory"); - - if (!this->client_ptr_->wait_for_action_server(5s)) { - RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); - rclcpp::shutdown(); - } + jt_publisher_ = this->create_publisher( + "/waist_yaw_controller/joint_trajectory", 10); } void WaistJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) { // 動作時間 - const double TIME_FROM_START = 1.0e-9; + 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); - - - // 動作完了していない場合はgoalを配信しない - if (!has_result_) { - return; - } // 角度指令値取得 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名設定 - auto goal_msg = control_msgs::action::FollowJointTrajectory::Goal(); - goal_msg.trajectory.joint_names.push_back("waist_yaw_joint"); + trajectory_msgs::msg::JointTrajectory jt_msg; + jt_msg.joint_names.push_back("waist_yaw_joint"); // 角度指令値設定 - trajectory_msgs::msg::JointTrajectoryPoint trajectory_point_msg; - trajectory_point_msg.positions.push_back(yaw_angle); - trajectory_point_msg.time_from_start = rclcpp::Duration::from_seconds(TIME_FROM_START); - goal_msg.trajectory.points.push_back(trajectory_point_msg); - - auto send_goal_options = - rclcpp_action::Client::SendGoalOptions(); - send_goal_options.result_callback = std::bind(&WaistJtControl::result_callback, this, _1); - - // 角度指令値配信 - this->client_ptr_->async_send_goal(goal_msg, send_goal_options); - has_result_ = false; -} - -void WaistJtControl::result_callback( - const GoalHandleJt::WrappedResult & result) -{ - switch (result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - return; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - return; - } + 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); - // 動作完了フラグをtrueにする - has_result_ = true; + jt_publisher_->publish(jt_msg); } } // namespace sciurus17_examples From 8fe900c81ac28dbb858af6923439b92d0135b824 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Mon, 18 Dec 2023 18:04:49 +0900 Subject: [PATCH 39/55] =?UTF-8?q?Topic=E5=90=8D=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/neck_jt_control.cpp | 14 +++++++------- sciurus17_examples/src/object_tracker.cpp | 21 ++++++++++++++++++--- 2 files changed, 25 insertions(+), 10 deletions(-) diff --git a/sciurus17_examples/src/neck_jt_control.cpp b/sciurus17_examples/src/neck_jt_control.cpp index de00f30..76fbe30 100644 --- a/sciurus17_examples/src/neck_jt_control.cpp +++ b/sciurus17_examples/src/neck_jt_control.cpp @@ -30,10 +30,10 @@ 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)); + "target_angles", 10, std::bind(&NeckJtControl::angles_callback, this, _1)); jt_publisher_ = this->create_publisher( - "neck_controller/joint_trajectory", 10); + "/neck_controller/joint_trajectory", 10); } void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) @@ -63,11 +63,11 @@ void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::Shar jt_msg.joint_names.push_back("neck_pitch_joint"); // 角度指令値設定 - trajectory_msgs::msg::JointTrajectoryPoint trajectory_point_msg; - trajectory_point_msg.positions.push_back(yaw_angle); - trajectory_point_msg.positions.push_back(pitch_angle); - trajectory_point_msg.time_from_start = rclcpp::Duration(TIME_FROM_START); - jt_msg.points.push_back(trajectory_point_msg); + 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); } diff --git a/sciurus17_examples/src/object_tracker.cpp b/sciurus17_examples/src/object_tracker.cpp index 1badca3..792ba4a 100644 --- a/sciurus17_examples/src/object_tracker.cpp +++ b/sciurus17_examples/src/object_tracker.cpp @@ -38,10 +38,10 @@ ObjectTracker::ObjectTracker(const rclcpp::NodeOptions & options) "/neck_controller/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)); + "target_position", 10, std::bind(&ObjectTracker::point_callback, this, _1)); angles_publisher_ = - this->create_publisher("/target_angles", 10); + this->create_publisher("target_angles", 10); } void ObjectTracker::state_callback( @@ -63,6 +63,12 @@ void ObjectTracker::tracking() // 角度指令値の初期値 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 double MAX_ANGULAR_DIFF = angles::from_degrees(1.5); @@ -84,12 +90,17 @@ void ObjectTracker::tracking() RCLCPP_INFO_STREAM(this->get_logger(), "Wating controller state."); return; } - if (current_angles_msg_->feedback.positions.size() != 2) { + if (current_angles_msg_->feedback.positions.size() != 1 + && current_angles_msg_->feedback.positions.size() != 2) { 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); + } } // 現在時刻取得 @@ -132,6 +143,10 @@ void ObjectTracker::tracking() } } + // 角度指令値を可動範囲内にする + 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]); From 1e4aafdfedd7eb9a74c8b686c2ce0d780a6023dd Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Mon, 18 Dec 2023 18:05:36 +0900 Subject: [PATCH 40/55] =?UTF-8?q?chest=5Fcamera=5Ftracking.launch.py?= =?UTF-8?q?=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/chest_camera_tracking.launch.py | 70 +++++++++++++++++++ 1 file changed, 70 insertions(+) create mode 100644 sciurus17_examples/launch/chest_camera_tracking.launch.py 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..50f5871 --- /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=[ + ('/head_camera/color/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=[ + ('/neck_controller/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 + ]) From dc9df1e24d8d2eb13846cebed338ee21382ab33f Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Mon, 18 Dec 2023 18:05:55 +0900 Subject: [PATCH 41/55] =?UTF-8?q?namespace=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/launch/head_camera_tracking.launch.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/sciurus17_examples/launch/head_camera_tracking.launch.py b/sciurus17_examples/launch/head_camera_tracking.launch.py index 367962c..963242c 100644 --- a/sciurus17_examples/launch/head_camera_tracking.launch.py +++ b/sciurus17_examples/launch/head_camera_tracking.launch.py @@ -28,7 +28,7 @@ def generate_launch_description(): container = ComposableNodeContainer( name='tracking_container', - namespace='', + namespace='head_camera_tracking', package='rclcpp_components', executable='component_container', composable_node_descriptions=[ @@ -36,18 +36,21 @@ def generate_launch_description(): package='sciurus17_examples', plugin='sciurus17_examples::ColorDetection', name='color_detection', + namespace='head_camera_tracking', extra_arguments=[{'use_intra_process_comms': True}] ), ComposableNode( package='sciurus17_examples', plugin='sciurus17_examples::ObjectTracker', name='object_tracker', + namespace='head_camera_tracking', 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}] ), ], From b297cc7b6b110d378e35da63473a75185bb097d8 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Mon, 18 Dec 2023 18:08:23 +0900 Subject: [PATCH 42/55] =?UTF-8?q?=E3=82=B9=E3=82=BF=E3=82=A4=E3=83=AB?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/object_tracker.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/sciurus17_examples/src/object_tracker.cpp b/sciurus17_examples/src/object_tracker.cpp index 792ba4a..cc5ba37 100644 --- a/sciurus17_examples/src/object_tracker.cpp +++ b/sciurus17_examples/src/object_tracker.cpp @@ -90,15 +90,15 @@ void ObjectTracker::tracking() RCLCPP_INFO_STREAM(this->get_logger(), "Wating controller state."); return; } - if (current_angles_msg_->feedback.positions.size() != 1 - && current_angles_msg_->feedback.positions.size() != 2) { + if (current_angles_msg_->feedback.positions.size() != 1 && + current_angles_msg_->feedback.positions.size() != 2) + { return; } const auto current_angles = current_angles_msg_->feedback.positions; if (target_angles_.empty()) { target_angles_ = current_angles; - if (target_angles_.size() == 1) - { + if (target_angles_.size() == 1) { target_angles_.push_back(0); } } From 6db42c20caab50d5e7b3acb891e3d9ff0ca8aba1 Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Mon, 18 Dec 2023 18:14:17 +0900 Subject: [PATCH 43/55] Update README --- sciurus17_examples/README.md | 29 ++++++++++++++++++++++++----- 1 file changed, 24 insertions(+), 5 deletions(-) diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index b983fef..58eb169 100644 --- a/sciurus17_examples/README.md +++ b/sciurus17_examples/README.md @@ -17,7 +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) - - [object\_tracking](#object_tracking) + - [head\_camera\_tracking](#head_camera_tracking) + - [chest\_camera\_tracking](#chest_camera_tracking) ## 準備（実機を使う場合） @@ -90,7 +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) -- [object\_tracking](#object_tracking) +- [head\_camera\_tracking](#head_camera_tracking) +- [chest\_camera\_tracking](#chest_camera_tracking) 実行できるサンプルの一覧は、`example.launch.py`にオプション`-s`を付けて実行することで表示できます。 @@ -168,9 +170,9 @@ ros2 launch sciurus17_examples example.launch.py example:='pick_and_place_left_a --- -### object_tracking +### head_camera_tracking -カメラ映像を用いてオレンジ色の物体を追従するコード例です。 +頭部カメラ映像を用いてオレンジ色の物体を追従するコード例です。 Gazeboで実行する場合は動作環境によってうまく追従しない場合があります。 カメラ解像度やサンプルコード内の追従速度ゲインを調整してください。 @@ -178,7 +180,24 @@ Gazeboで実行する場合は動作環境によってうまく追従しない 次のコマンドを実行します。 ```sh -ros2 launch sciurus17_examples object_tracking.launch.py +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) From a1d5af78a8e3f026c3d92c3de5fbbd7907f4c574 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Tue, 26 Dec 2023 16:14:56 +0900 Subject: [PATCH 44/55] =?UTF-8?q?=E8=83=B8=E9=83=A8=E3=82=AB=E3=83=A1?= =?UTF-8?q?=E3=83=A9=E3=83=91=E3=82=B9=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_vision/launch/chest_camera.launch.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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}, From 5f71ddfba478945a27e8d25800a32e4dc0e3579a Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Tue, 26 Dec 2023 16:17:57 +0900 Subject: [PATCH 45/55] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AA=E5=87=A6?= =?UTF-8?q?=E7=90=86=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/CMakeLists.txt | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index 68c3651..1da0016 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/sciurus17_examples/CMakeLists.txt @@ -24,8 +24,6 @@ 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(pcl_conversions REQUIRED) -find_package(pcl_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(rclcpp_action REQUIRED) @@ -57,16 +55,9 @@ foreach(loop_var IN LISTS components_list) endforeach() rclcpp_components_register_nodes(color_detection "sciurus17_examples::ColorDetection") -set(node_plgins "${node_plugins}sciurus17_examples::ColorDetection;$\n") - rclcpp_components_register_nodes(neck_jt_control "sciurus17_examples::NeckJtControl") -set(node_plgins "${node_plugins}sciurus17_examples::NeckJtControl;$\n") - rclcpp_components_register_nodes(object_tracker "sciurus17_examples::ObjectTracker") -set(node_plgins "${node_plugins}sciurus17_examples::ObjectTracker;$\n") - rclcpp_components_register_nodes(waist_jt_control "sciurus17_examples::WaistJtControl") -set(node_plgins "${node_plugins}sciurus17_examples::WaistJtControl;$\n") install(TARGETS color_detection @@ -135,8 +126,6 @@ foreach(loop_var IN LISTS executable_list) image_geometry moveit_ros_planning_interface OpenCV - pcl_ros - pcl_conversions rclcpp tf2_geometry_msgs control_msgs From 3264c141f8b0e936b72dfb6ce2f9e82dd685dc2d Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Tue, 26 Dec 2023 16:23:38 +0900 Subject: [PATCH 46/55] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AA=E3=83=91?= =?UTF-8?q?=E3=83=83=E3=82=B1=E3=83=BC=E3=82=B8=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/package.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/sciurus17_examples/package.xml b/sciurus17_examples/package.xml index 6339169..50c7f53 100644 --- a/sciurus17_examples/package.xml +++ b/sciurus17_examples/package.xml @@ -24,10 +24,8 @@ image_geometry libopencv-dev moveit_ros_planning_interface - pcl_ros rclcpp rclcpp_components - realsense2_camera tf2_geometry_msgs ament_lint_auto From 69d9ca0ac276aba78fa6d4742961dd680f6d1a21 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Tue, 26 Dec 2023 16:30:30 +0900 Subject: [PATCH 47/55] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AA=E3=82=B3?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E3=81=AE=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/color_detection.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index 0095f15..d7e7136 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -43,12 +43,10 @@ ColorDetection::ColorDetection(const rclcpp::NodeOptions & options) 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; - // ウェブカメラの画像を受け取る auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); // 画像をRGBからHSVに変換 From dc8379928a4d86a1b2e2d50491c3622e856b7c48 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Tue, 26 Dec 2023 16:36:16 +0900 Subject: [PATCH 48/55] =?UTF-8?q?=E8=85=B0=E3=80=81=E9=A6=96=E3=81=AE?= =?UTF-8?q?=E9=96=A2=E7=AF=80=E6=95=B0=E3=82=92=E5=A4=89=E6=95=B0=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/object_tracker.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/sciurus17_examples/src/object_tracker.cpp b/sciurus17_examples/src/object_tracker.cpp index cc5ba37..08f2b94 100644 --- a/sciurus17_examples/src/object_tracker.cpp +++ b/sciurus17_examples/src/object_tracker.cpp @@ -69,6 +69,10 @@ void ObjectTracker::tracking() 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); @@ -90,8 +94,8 @@ void ObjectTracker::tracking() RCLCPP_INFO_STREAM(this->get_logger(), "Wating controller state."); return; } - if (current_angles_msg_->feedback.positions.size() != 1 && - current_angles_msg_->feedback.positions.size() != 2) + if (current_angles_msg_->feedback.positions.size() != WAIST_JOINT_NUM && + current_angles_msg_->feedback.positions.size() != NECK_JOINT_NUM) { return; } From e58f2a1ef0c031b7482a77341a4c07d046715f83 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Tue, 26 Dec 2023 16:44:21 +0900 Subject: [PATCH 49/55] =?UTF-8?q?RCLCPP=5FINFO=E3=82=92DEBUG=E3=81=AB?= =?UTF-8?q?=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/color_detection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index d7e7136..4098c41 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -94,7 +94,7 @@ void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg object_point.x = d_m10 / d_area; object_point.y = d_m01 / d_area; - RCLCPP_INFO_STREAM(this->get_logger(), "Detect at" << object_point << "."); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Detect at" << object_point << "."); // 画像の中心を原点とした検出位置に変換 cv::Point2d translated_object_point; From d2e375758c49f046e571709c69d14efa00ff3aa0 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Tue, 9 Jan 2024 11:24:34 +0900 Subject: [PATCH 50/55] =?UTF-8?q?topic=E5=90=8D=E3=82=92=E6=8A=BD=E8=B1=A1?= =?UTF-8?q?=E7=9A=84=E3=81=AA=E3=82=82=E3=81=AE=E3=81=AB=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/launch/chest_camera_tracking.launch.py | 4 ++-- sciurus17_examples/launch/head_camera_tracking.launch.py | 6 ++++++ sciurus17_examples/src/color_detection.cpp | 2 +- sciurus17_examples/src/object_tracker.cpp | 2 +- 4 files changed, 10 insertions(+), 4 deletions(-) diff --git a/sciurus17_examples/launch/chest_camera_tracking.launch.py b/sciurus17_examples/launch/chest_camera_tracking.launch.py index 50f5871..03d9656 100644 --- a/sciurus17_examples/launch/chest_camera_tracking.launch.py +++ b/sciurus17_examples/launch/chest_camera_tracking.launch.py @@ -38,7 +38,7 @@ def generate_launch_description(): name='color_detection', namespace='chest_camera_tracking', remappings=[ - ('/head_camera/color/image_raw', '/chest_camera/image_raw') + ('/image_raw', '/chest_camera/image_raw') ], extra_arguments=[{'use_intra_process_comms': True}] ), @@ -48,7 +48,7 @@ def generate_launch_description(): name='object_tracker', namespace='chest_camera_tracking', remappings=[ - ('/neck_controller/controller_state', '/waist_yaw_controller/controller_state') + ('/controller_state', '/waist_yaw_controller/controller_state') ], extra_arguments=[{'use_intra_process_comms': True}] ), diff --git a/sciurus17_examples/launch/head_camera_tracking.launch.py b/sciurus17_examples/launch/head_camera_tracking.launch.py index 963242c..24373b7 100644 --- a/sciurus17_examples/launch/head_camera_tracking.launch.py +++ b/sciurus17_examples/launch/head_camera_tracking.launch.py @@ -37,6 +37,9 @@ def generate_launch_description(): 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( @@ -51,6 +54,9 @@ def generate_launch_description(): plugin='sciurus17_examples::NeckJtControl', name='neck_jt_control', namespace='head_camera_tracking', + remappings=[ + ('/controller_state', '/neck_controller/controller_state') + ], extra_arguments=[{'use_intra_process_comms': True}] ), ], diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index 4098c41..6be5617 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -31,7 +31,7 @@ ColorDetection::ColorDetection(const rclcpp::NodeOptions & options) : Node("color_detection", options) { image_subscription_ = this->create_subscription( - "/head_camera/color/image_raw", 10, std::bind(&ColorDetection::image_callback, this, _1)); + "/image_raw", 10, std::bind(&ColorDetection::image_callback, this, _1)); image_thresholded_publisher_ = this->create_publisher("image_thresholded", 10); diff --git a/sciurus17_examples/src/object_tracker.cpp b/sciurus17_examples/src/object_tracker.cpp index 08f2b94..d8409ef 100644 --- a/sciurus17_examples/src/object_tracker.cpp +++ b/sciurus17_examples/src/object_tracker.cpp @@ -35,7 +35,7 @@ ObjectTracker::ObjectTracker(const rclcpp::NodeOptions & options) state_subscription_ = this->create_subscription( - "/neck_controller/controller_state", 10, std::bind(&ObjectTracker::state_callback, this, _1)); + "/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)); From 1a6d958145c05023da87dda3c2afc0e24c94fc3f Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Thu, 11 Jan 2024 19:41:55 +0900 Subject: [PATCH 51/55] =?UTF-8?q?=E4=B8=80=E7=95=AA=E5=A4=A7=E3=81=8D?= =?UTF-8?q?=E3=81=8F=E6=98=A0=E3=81=A3=E3=81=9F=E9=A0=98=E5=9F=9F=E3=81=AE?= =?UTF-8?q?=E3=81=BF=E3=82=92=E6=A4=9C=E5=87=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sciurus17_examples/color_detection.hpp | 2 +- sciurus17_examples/src/color_detection.cpp | 107 +++++++++++------- 2 files changed, 66 insertions(+), 43 deletions(-) diff --git a/sciurus17_examples/include/sciurus17_examples/color_detection.hpp b/sciurus17_examples/include/sciurus17_examples/color_detection.hpp index abdc7af..8d4dfc6 100644 --- a/sciurus17_examples/include/sciurus17_examples/color_detection.hpp +++ b/sciurus17_examples/include/sciurus17_examples/color_detection.hpp @@ -30,7 +30,7 @@ class ColorDetection : public rclcpp::Node private: rclcpp::Subscription::SharedPtr image_subscription_; - rclcpp::Publisher::SharedPtr image_thresholded_publisher_; + rclcpp::Publisher::SharedPtr image_annotated_publisher_; rclcpp::Publisher::SharedPtr object_point_publisher_; rclcpp::TimerBase::SharedPtr timer_; diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index 6be5617..f0178e0 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -14,6 +14,7 @@ // 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" @@ -33,8 +34,8 @@ ColorDetection::ColorDetection(const rclcpp::NodeOptions & options) image_subscription_ = this->create_subscription( "/image_raw", 10, std::bind(&ColorDetection::image_callback, this, _1)); - image_thresholded_publisher_ = - this->create_publisher("image_thresholded", 10); + image_annotated_publisher_ = + this->create_publisher("image_annotated", 10); object_point_publisher_ = this->create_publisher("target_position", 10); @@ -47,17 +48,19 @@ void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg 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::cvtColor(cv_img->image, cv_img->image, cv::COLOR_RGB2HSV); - - // 画像処理用の変数を用意 - cv::Mat img_thresholded; + cv::Mat img_hsv; + cv::cvtColor(cv_img->image, img_hsv, cv::COLOR_RGB2HSV); // 画像の二値化 + cv::Mat img_thresholded; cv::inRange( - cv_img->image, + img_hsv, cv::Scalar(LOW_H, LOW_S, LOW_V), cv::Scalar(HIGH_H, HIGH_S, HIGH_V), img_thresholded); @@ -76,45 +79,65 @@ void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg cv::MORPH_CLOSE, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); - // 閾値による二値化画像を配信 - sensor_msgs::msg::Image::SharedPtr img_thresholded_msg = - cv_bridge::CvImage(msg->header, "mono8", img_thresholded).toImageMsg(); - image_thresholded_publisher_->publish(*img_thresholded_msg); - - // 画像の検出領域におけるモーメントを計算 - cv::Moments moment = moments(img_thresholded); - double d_m01 = moment.m01; - double d_m10 = moment.m10; - double d_area = moment.m00; - - // 検出領域のピクセル数が100000より大きい場合 - if (d_area > 100000) { + // 検出領域のみを描画 + 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 objects_moments; + int max_area_i = 0; + int i = 0; + for (const auto & contour : contours) { + objects_moments.push_back(cv::moments(contour)); + if (objects_moments[max_area_i].m00 < objects_moments[i].m00) { + max_area_i = i; + } + i++; + } + // 画像座標系における物体検出位置（2D） cv::Point2d object_point; - object_point.x = d_m10 / d_area; - object_point.y = d_m01 / d_area; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "Detect at" << object_point << "."); - - // 画像の中心を原点とした検出位置に変換 - 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); + object_point.x = objects_moments[max_area_i].m10 / objects_moments[max_area_i].m00; + object_point.y = objects_moments[max_area_i].m01 / objects_moments[max_area_i].m00; + + if (objects_moments[max_area_i].m00 > MIN_OBJECT_SIZE) { + RCLCPP_DEBUG_STREAM(this->get_logger(), "Detect at" << object_point << "."); + + // 検出領域と検出位置を描画 + cv::Scalar color(256, 0, 256); + cv::drawContours(img_annotated, contours, max_area_i, color, 2); + cv::circle(img_annotated, object_point, 10, 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); } - - // 検出位置を配信 - 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 From 819c418d4167e5d685b05e50317e18dbdc19c058 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Thu, 11 Jan 2024 19:42:26 +0900 Subject: [PATCH 52/55] =?UTF-8?q?remap=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/launch/head_camera_tracking.launch.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sciurus17_examples/launch/head_camera_tracking.launch.py b/sciurus17_examples/launch/head_camera_tracking.launch.py index 24373b7..4ca5c6b 100644 --- a/sciurus17_examples/launch/head_camera_tracking.launch.py +++ b/sciurus17_examples/launch/head_camera_tracking.launch.py @@ -47,6 +47,9 @@ def generate_launch_description(): 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( @@ -54,9 +57,6 @@ def generate_launch_description(): plugin='sciurus17_examples::NeckJtControl', name='neck_jt_control', namespace='head_camera_tracking', - remappings=[ - ('/controller_state', '/neck_controller/controller_state') - ], extra_arguments=[{'use_intra_process_comms': True}] ), ], From d780339f8df674d42e7990329217296b00635f46 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Thu, 11 Jan 2024 19:42:39 +0900 Subject: [PATCH 53/55] =?UTF-8?q?=E8=AA=A4=E5=AD=97=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/object_tracker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples/src/object_tracker.cpp b/sciurus17_examples/src/object_tracker.cpp index d8409ef..b994d59 100644 --- a/sciurus17_examples/src/object_tracker.cpp +++ b/sciurus17_examples/src/object_tracker.cpp @@ -91,7 +91,7 @@ void ObjectTracker::tracking() // 現在の関節角度を取得 if (!current_angles_msg_) { - RCLCPP_INFO_STREAM(this->get_logger(), "Wating controller state."); + RCLCPP_INFO_STREAM(this->get_logger(), "Waiting controller state."); return; } if (current_angles_msg_->feedback.positions.size() != WAIST_JOINT_NUM && From 9b46ee1da48e2c41a19c6700cd1f5459e1724cd7 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Thu, 11 Jan 2024 20:09:49 +0900 Subject: [PATCH 54/55] =?UTF-8?q?=E3=83=91=E3=83=A9=E3=83=A1=E3=83=BC?= =?UTF-8?q?=E3=82=BF=E8=AA=BF=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/color_detection.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index f0178e0..a5386b9 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -109,9 +109,11 @@ void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg RCLCPP_DEBUG_STREAM(this->get_logger(), "Detect at" << object_point << "."); // 検出領域と検出位置を描画 - cv::Scalar color(256, 0, 256); - cv::drawContours(img_annotated, contours, max_area_i, color, 2); - cv::circle(img_annotated, object_point, 10, color, -1); + 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; From 19f7faa5f6c69a56e521d378cf639a70522d92f0 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Fri, 12 Jan 2024 19:42:53 +0900 Subject: [PATCH 55/55] =?UTF-8?q?=E5=A4=89=E6=95=B0=E5=90=8D=E5=A4=89?= =?UTF-8?q?=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/color_detection.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index a5386b9..cf93027 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -89,23 +89,23 @@ void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg if (contours.size()) { // 最も面積の大きい領域を取得 - std::vector objects_moments; - int max_area_i = 0; + std::vector object_moments; + int max_area_i = -1; int i = 0; for (const auto & contour : contours) { - objects_moments.push_back(cv::moments(contour)); - if (objects_moments[max_area_i].m00 < objects_moments[i].m00) { + object_moments.push_back(cv::moments(contour)); + if (object_moments[max_area_i].m00 < object_moments[i].m00) { max_area_i = i; } i++; } - // 画像座標系における物体検出位置（2D） - cv::Point2d object_point; - object_point.x = objects_moments[max_area_i].m10 / objects_moments[max_area_i].m00; - object_point.y = objects_moments[max_area_i].m01 / objects_moments[max_area_i].m00; + 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; - if (objects_moments[max_area_i].m00 > MIN_OBJECT_SIZE) { RCLCPP_DEBUG_STREAM(this->get_logger(), "Detect at" << object_point << "."); // 検出領域と検出位置を描画