From acee967a7df003a40d25ab7fd0d69a8df5d15e4f Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Tue, 7 Nov 2023 15:08:41 +0900 Subject: [PATCH] =?UTF-8?q?2.1.0=E3=83=AA=E3=83=AA=E3=83=BC=E3=82=B9?= =?UTF-8?q?=E3=81=AE=E3=81=9F=E3=82=81=E3=81=ABmaster=E3=81=AE=E5=A4=89?= =?UTF-8?q?=E6=9B=B4=E5=B7=AE=E5=88=86=E3=82=92humble-devel=E3=81=B8?= =?UTF-8?q?=E3=83=9E=E3=83=BC=E3=82=B8=20(#46)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * object_trackingにおいて画像トピックをサブスクライブするように変更 (#43) * 画像トピックのサブスクライバを実装 * 画像トピックがRGBなのでBGRに変換 * mouseとcamera_nodeのオンオフを実装 * 実機で動作するように変更 * 物体追従のパラメータ調整 * 不要なメンバ変数を削除 * コールバック関数の名前を変更 * コメントアウトしていた不要な行を削除 * rclcpp::SensorDataQoS()を使用するように変更 * std::placeholders::_1を直接書くように修正 * raspimouseノードにComposableNodeを使用 * usb_camノードにもComposableNodeを使用 * 各ノードのuse_intra_process_commsをtrueに変更 * image_pub_を削除 * READMEのobject_trackingのコマンドを修正 * Gazeboの場合のコマンド例を削除 * READMEにGazeboでも実行できることを追記 (#44) * READMEにGazeboでも動作することを追記 * 英語版のREADMEにもGazeboの情報を追記 * READMEのサンプル集のリンクを修正 * READMEのraspimouse_simのリンクを修正 * リリースのためにCHANGELOG.rstとpackage.xmlを更新 (#45) * リリースのためにCHANGELOG.rstを更新 * 2.1.0 --- CHANGELOG.rst | 6 +++ CMakeLists.txt | 4 +- README.en.md | 8 ++-- README.md | 8 ++-- .../object_tracking_component.hpp | 8 +--- launch/object_tracking.launch.py | 47 +++++++++++++++++-- package.xml | 3 +- src/object_tracking_component.cpp | 41 +++++----------- 8 files changed, 78 insertions(+), 47 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 59f9cfc..ea7813e 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package raspimouse_ros2_examples ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.0 (2023-11-07) +------------------ +* READMEにGazeboでも実行できることを追記 (`#44 `_) +* object_trackingにおいて画像トピックをサブスクライブするように変更 (`#43 `_) +* Contributors: YusukeKato + 2.0.0 (2023-08-03) ------------------ * Humble対応 (`#41 `_) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9515133..c6238bb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,6 +30,7 @@ find_package(OpenCV REQUIRED) find_package(raspimouse_msgs REQUIRED) find_package(raspimouse REQUIRED) find_package(rt_usb_9axisimu_driver REQUIRED) +find_package(cv_bridge REQUIRED) include_directories(include) @@ -46,7 +47,8 @@ ament_target_dependencies(object_tracking_component std_srvs sensor_msgs geometry_msgs - OpenCV) + OpenCV + cv_bridge) rclcpp_components_register_nodes(object_tracking_component "object_tracking::Tracker") add_library(line_follower_component SHARED diff --git a/README.en.md b/README.en.md index d293e46..e586116 100644 --- a/README.en.md +++ b/README.en.md @@ -6,7 +6,9 @@ ROS 2 examples for Raspberry Pi Mouse. -ROS1 examples is [here](https://github.com/rt-net/raspimouse_ros_examples). +ROS1 examples is [here](https://github.com/rt-net/raspimouse_ros_examples/blob/master/README.en.md). + +To run on Gazebo, click [here](https://github.com/rt-net/raspimouse_sim/blob/ros2/README.en.md). @@ -155,10 +157,10 @@ $ ./configure_camera.bash Then, launch nodes with the following command: ```sh -$ ros2 launch raspimouse_ros2_examples object_tracking.launch.py +$ ros2 launch raspimouse_ros2_examples object_tracking.launch.py video_device:=/dev/video0 ``` -This sample publishes two topics: `raw_image` for the camera image and `result_image` for the object detection image. +This sample publishes two topics: `camera/color/image_raw` for the camera image and `result_image` for the object detection image. These images can be viewed with [RViz](https://index.ros.org/r/rviz/) or [rqt_image_view](https://index.ros.org/doc/ros2/Tutorials/RQt-Overview-Usage/). diff --git a/README.md b/README.md index 7783b80..2b82003 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,9 @@ Raspberry Pi MouseのROS 2サンプルコード集です。 -ROS1のサンプルコード集は[こちら](https://github.com/rt-net/raspimouse_ros_examples)。 +ROS1のサンプルコード集は[こちら](https://github.com/rt-net/raspimouse_ros_examples/blob/master/README.md)。 + +Gazebo（シミュレータ）でも動作します。詳細は[こちら](https://github.com/rt-net/raspimouse_sim/blob/ros2/README.md)。 @@ -157,10 +159,10 @@ $ ./configure_camera.bash 次のコマンドでノードを起動します。 ```sh -$ ros2 launch raspimouse_ros2_examples object_tracking.launch.py +$ ros2 launch raspimouse_ros2_examples object_tracking.launch.py video_device:=/dev/video0 ``` -カメラ画像は`raw_image`、物体検出画像は`result_image`というトピックとして発行されます。 +カメラ画像は`camera/color/image_raw`、物体検出画像は`result_image`というトピックとして発行されます。 これらの画像は[RViz](https://index.ros.org/r/rviz/) や[rqt_image_view](https://index.ros.org/doc/ros2/Tutorials/RQt-Overview-Usage/) で表示できます。 diff --git a/include/raspimouse_ros2_examples/object_tracking_component.hpp b/include/raspimouse_ros2_examples/object_tracking_component.hpp index 3524059..7c8dc9c 100644 --- a/include/raspimouse_ros2_examples/object_tracking_component.hpp +++ b/include/raspimouse_ros2_examples/object_tracking_component.hpp @@ -38,23 +38,19 @@ class Tracker : public rclcpp_lifecycle::LifecycleNode explicit Tracker(const rclcpp::NodeOptions & options); protected: - void on_image_timer(); + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg_image); void on_cmd_vel_timer(); private: cv::VideoCapture cap_; - int device_index_; - double image_width_; - double image_height_; bool object_is_detected_; cv::Point2d object_normalized_point_; double object_normalized_area_; geometry_msgs::msg::Twist cmd_vel_; - std::shared_ptr> image_pub_; std::shared_ptr> result_image_pub_; std::shared_ptr> cmd_vel_pub_; std::shared_ptr> motor_power_client_; - rclcpp::TimerBase::SharedPtr image_timer_; + rclcpp::Subscription::SharedPtr image_sub_; rclcpp::TimerBase::SharedPtr cmd_vel_timer_; std::string mat_type2encoding(int mat_type); diff --git a/launch/object_tracking.launch.py b/launch/object_tracking.launch.py index 183c1e6..e29a51a 100644 --- a/launch/object_tracking.launch.py +++ b/launch/object_tracking.launch.py @@ -13,12 +13,31 @@ # limitations under the License. import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode def generate_launch_description(): + declare_mouse = DeclareLaunchArgument( + 'mouse', + default_value="true", + description='Launch raspimouse node' + ) + declare_use_camera_node = DeclareLaunchArgument( + 'use_camera_node', + default_value='true', + description='Use camera node.' + ) + declare_video_device = DeclareLaunchArgument( + 'video_device', + default_value='/dev/video0', + description='Set video device.' + ) + """Generate launch description with multiple components.""" container = ComposableNodeContainer( name='object_tracking_container', @@ -29,12 +48,27 @@ def generate_launch_description(): ComposableNode( package='raspimouse_ros2_examples', plugin='object_tracking::Tracker', - name='tracker'), + name='tracker', + extra_arguments=[{'use_intra_process_comms': True}]), ComposableNode( package='raspimouse', plugin='raspimouse::Raspimouse', name='raspimouse', - parameters=[{'use_light_sensors': False}]), + parameters=[{'use_light_sensors': False}], + extra_arguments=[{'use_intra_process_comms': True}], + condition=IfCondition(LaunchConfiguration('mouse'))), + ComposableNode( + package='usb_cam', + plugin='usb_cam::UsbCamNode', + name='usb_cam', + remappings=[('image_raw', 'camera/color/image_raw')], + parameters=[ + {'video_device': LaunchConfiguration('video_device')}, + {'frame_id': 'camera_color_optical_frame'}, + {'pixel_format': 'yuyv2rgb'} + ], + extra_arguments=[{'use_intra_process_comms': True}], + condition=IfCondition(LaunchConfiguration('use_camera_node'))), ], output='screen', ) @@ -45,7 +79,12 @@ def generate_launch_description(): executable='lifecycle_node_manager', output='screen', parameters=[{'components': ['raspimouse', 'tracker']}] - ) - return launch.LaunchDescription([container, manager]) + return launch.LaunchDescription([ + declare_mouse, + declare_use_camera_node, + declare_video_device, + container, + manager + ]) diff --git a/package.xml b/package.xml index fe749fb..f634699 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ raspimouse_ros2_examples - 2.0.0 + 2.1.0 Raspberry Pi Mouse examples RT Corporation @@ -30,6 +30,7 @@ hls_lfcd_lds_driver nav2_map_server slam_toolbox + cv_bridge ament_lint_auto ament_lint_common diff --git a/src/object_tracking_component.cpp b/src/object_tracking_component.cpp index cc10600..0673839 100644 --- a/src/object_tracking_component.cpp +++ b/src/object_tracking_component.cpp @@ -25,6 +25,7 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" #include "lifecycle_msgs/srv/change_state.hpp" +#include "cv_bridge/cv_bridge.h" using namespace std::chrono_literals; using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -34,28 +35,25 @@ namespace object_tracking Tracker::Tracker(const rclcpp::NodeOptions & options) : rclcpp_lifecycle::LifecycleNode("tracker", options), - device_index_(0), image_width_(320), image_height_(240), object_is_detected_(false) { } -void Tracker::on_image_timer() +void Tracker::image_callback(const sensor_msgs::msg::Image::SharedPtr msg_image) { + auto cv_img = cv_bridge::toCvShare(msg_image, msg_image->encoding); auto msg = std::make_unique(); auto result_msg = std::make_unique(); msg->is_bigendian = false; result_msg->is_bigendian = false; cv::Mat frame, result_frame; - cap_ >> frame; + cv::cvtColor(cv_img->image, frame, CV_RGB2BGR); if (!frame.empty()) { tracking(frame, result_frame); convert_frame_to_message(result_frame, *result_msg); result_image_pub_->publish(std::move(result_msg)); - - convert_frame_to_message(frame, *msg); - image_pub_->publish(std::move(msg)); } } @@ -63,8 +61,8 @@ void Tracker::on_cmd_vel_timer() { const double LINEAR_VEL = -0.5; // unit: m/s const double ANGULAR_VEL = -0.8; // unit: rad/s - const double TARGET_AREA = 0.3; // 0.0 ~ 1.0 - const double OBJECT_AREA_THRESHOLD = 0.06; // 0.0 ~ 1.0 + const double TARGET_AREA = 0.1; // 0.0 ~ 1.0 + const double OBJECT_AREA_THRESHOLD = 0.01; // 0.0 ~ 1.0 // Detects an object and tracks it // when the number of pixels of the object is greater than the threshold. @@ -117,7 +115,7 @@ void Tracker::tracking(const cv::Mat & input_frame, cv::Mat & result_frame) cv::Mat hsv; cv::cvtColor(input_frame, hsv, cv::COLOR_BGR2HSV); cv::Mat extracted_bin; - cv::inRange(hsv, cv::Scalar(9, 100, 100), cv::Scalar(29, 255, 255), extracted_bin); // Orange + cv::inRange(hsv, cv::Scalar(0, 100, 100), cv::Scalar(29, 255, 255), extracted_bin); // Red-Orange // cv::inRange(hsv, cv::Scalar(60, 100, 100), cv::Scalar(80, 255, 255), extracted_bin); // Green // cv::inRange(hsv, cv::Scalar(100, 100, 100), cv::Scalar(120, 255, 255), extracted_bin); // Blue input_frame.copyTo(result_frame, extracted_bin); @@ -173,24 +171,15 @@ CallbackReturn Tracker::on_configure(const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "on_configure() is called."); - image_timer_ = create_wall_timer(100ms, std::bind(&Tracker::on_image_timer, this)); cmd_vel_timer_ = create_wall_timer(50ms, std::bind(&Tracker::on_cmd_vel_timer, this)); // Don't actually start publishing data until activated - image_timer_->cancel(); cmd_vel_timer_->cancel(); - image_pub_ = create_publisher("raw_image", 1); result_image_pub_ = create_publisher("result_image", 1); cmd_vel_pub_ = create_publisher("cmd_vel", 1); - - // Initialize OpenCV video capture stream. - cap_.open(device_index_); - cap_.set(cv::CAP_PROP_FRAME_WIDTH, image_width_); - cap_.set(cv::CAP_PROP_FRAME_HEIGHT, image_height_); - if (!cap_.isOpened()) { - RCLCPP_ERROR(this->get_logger(), "Could not open video stream"); - return CallbackReturn::FAILURE; - } + image_sub_ = create_subscription( + "camera/color/image_raw", rclcpp::SensorDataQoS(), + std::bind(&Tracker::image_callback, this, std::placeholders::_1)); return CallbackReturn::SUCCESS; } @@ -210,10 +199,8 @@ CallbackReturn Tracker::on_activate(const rclcpp_lifecycle::State &) request->data = true; auto future_result = motor_power_client_->async_send_request(request); - image_pub_->on_activate(); result_image_pub_->on_activate(); cmd_vel_pub_->on_activate(); - image_timer_->reset(); cmd_vel_timer_->reset(); return CallbackReturn::SUCCESS; @@ -222,10 +209,8 @@ CallbackReturn Tracker::on_activate(const rclcpp_lifecycle::State &) CallbackReturn Tracker::on_deactivate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "on_deactivate() is called."); - image_pub_->on_deactivate(); result_image_pub_->on_deactivate(); cmd_vel_pub_->on_deactivate(); - image_timer_->cancel(); cmd_vel_timer_->cancel(); object_is_detected_ = false; @@ -238,11 +223,10 @@ CallbackReturn Tracker::on_cleanup(const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "on_cleanup() is called."); - image_pub_.reset(); result_image_pub_.reset(); cmd_vel_pub_.reset(); - image_timer_.reset(); cmd_vel_timer_.reset(); + image_sub_.reset(); return CallbackReturn::SUCCESS; } @@ -251,11 +235,10 @@ CallbackReturn Tracker::on_shutdown(const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "on_shutdown() is called."); - image_pub_.reset(); result_image_pub_.reset(); cmd_vel_pub_.reset(); - image_timer_.reset(); cmd_vel_timer_.reset(); + image_sub_.reset(); return CallbackReturn::SUCCESS; }