object_trackingにおいて画像トピックをサブスクライブするように変更 #43

YusukeKato · 2023-10-27T04:39:44Z

What does this implement/fix?

元々object_trackingではOpenCVを使用してRGBカメラから直接画像を取得していましたが、画像トピックをサブスクライブするように変更します。

実機ではusb_camを使用して画像トピックを配信します。

シミュレータではxacroファイルにカメラ情報を記述することで画像トピックを配信します。

Does this close any currently open issues?

しません。

How has this been tested?

シミュレータの場合

端末1で下記のコマンドを実行します。

ros2 launch raspimouse_gazebo raspimouse_with_color_objects.launch.py use_rgb_camera:=true

端末2で下記のコマンドを実行します。

ros2 launch raspimouse_ros2_examples object_tracking.launch.py mouse:=false use_camera_node:=false

Gazebo上の赤いキューブを移動させると、ラズパイマウスが追従します。

実機の場合

デバイスドライバのインストールを行ってから、端末で下記のコマンドを実行します。

ros2 launch raspimouse_ros2_examples object_tracking.launch.py video_device:=/dev/video0

ラズパイマウスが赤い物体を追従します。

Any other comments?

raspimouse_descriptionとraspimouse_simが必要です（両方ともros2ブランチ）。

https://github.com/rt-net/raspimouse_sim/tree/ros2

https://github.com/rt-net/raspimouse_description/tree/ros2

Checklists

I have read the CONTRIBUTING guidelines.
- If there is no guideline in the repository, rt-net organization's guideline applies.
I have checked to ensure there aren't other open Pull Requests for the same change.

ShotaAk

シミュレータ上での動作確認ができました。

いくつかコメントしているので確認お願いします。

ShotaAk · 2023-10-31T08:50:17Z

src/object_tracking_component.cpp

@@ -117,7 +120,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


オレンジ色（RGB: 1.0, 0.65, 0.0）でも認識することを確認しました。

ShotaAk · 2023-10-31T08:55:16Z

src/object_tracking_component.cpp

メンバ変数device_index_、image_witdh_、image_heightは使用されなくなったので削除してください

ShotaAk · 2023-10-31T08:57:18Z

include/raspimouse_ros2_examples/object_tracking_component.hpp

@@ -38,7 +38,7 @@ class Tracker : public rclcpp_lifecycle::LifecycleNode
  explicit Tracker(const rclcpp::NodeOptions & options);

 protected:
-  void on_image_timer();
+  void on_image_timer(const sensor_msgs::msg::Image::SharedPtr msg_image);


ここはコールバック関数なので、image_callbackのような名称に変更してください

ShotaAk · 2023-10-31T08:57:49Z

include/raspimouse_ros2_examples/object_tracking_component.hpp

@@ -54,7 +54,8 @@ class Tracker : public rclcpp_lifecycle::LifecycleNode
  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::Image>> result_image_pub_;
  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>> cmd_vel_pub_;
  std::shared_ptr<rclcpp::Client<std_srvs::srv::SetBool>> motor_power_client_;
-  rclcpp::TimerBase::SharedPtr image_timer_;
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
+  // rclcpp::TimerBase::SharedPtr image_timer_;


image_timer_は使用しないので削除してください

ShotaAk · 2023-10-31T09:14:46Z

src/object_tracking_component.cpp

-    return CallbackReturn::FAILURE;
-  }
+  image_sub_ = create_subscription<sensor_msgs::msg::Image>(
+    "camera/color/image_raw", 1, std::bind(&Tracker::on_image_timer, this, _1));


ROS 2のデモパッケージに倣って、rclcpp::SensorDataQoS()を使用してください

https://github.com/ros2/demos/blob/647f858a80cefa5bf205b0b48c255b28dd18a7a1/image_tools/src/cam2image.cpp#L94-L95

ShotaAk · 2023-10-31T09:21:31Z

src/object_tracking_component.cpp

@@ -25,6 +25,8 @@
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
 #include "lifecycle_msgs/srv/change_state.hpp"
+#include "cv_bridge/cv_bridge.h"
+using std::placeholders::_1;


細かい指摘です。
1箇所でしか使用してないので、create_subscription内に直接std::placeholders::_1と記述してください。

ShotaAk · 2023-10-31T09:35:36Z

launch/object_tracking.launch.py

-                ComposableNode(
-                    package='raspimouse',
-                    plugin='raspimouse::Raspimouse',
-                    name='raspimouse',
-                    parameters=[{'use_light_sensors': False}]),


ComposableNodeのまま使用できませんか？
IfConditionも適用できると思います。

https://github.com/ros2/launch_ros/blob/rolling/launch_ros/launch_ros/descriptions/composable_node.py#L48

ShotaAk · 2023-10-31T09:47:05Z

launch/object_tracking.launch.py

+    usb_cam_node = Node(
+        package='usb_cam',
+        executable='usb_cam_node_exe',
+        remappings=[('image_raw', 'camera/color/image_raw')],
+        parameters=[
+            {'video_device': LaunchConfiguration('video_device')},
+            {'frame_id': 'camera_color_optical_frame'},
+            {'pixel_format': 'yuyv2rgb'}
+        ],
+        condition=IfCondition(LaunchConfiguration('use_camera_node'))


usb_camもcomponentが提供されているので、composable nodeとして立ち上げてください。

usb_cam::UsbCamNode

参考情報：
https://docs.ros.org/en/foxy/Tutorials/Intermediate/Composition.html#discover-available-components
https://github.com/ros-drivers/usb_cam/blob/ros2/CMakeLists.txt#L62-L65

ShotaAk · 2023-10-31T09:49:05Z

launch/object_tracking.launch.py

+        parameters=[{'use_light_sensors': False, }],
+        condition=IfCondition(LaunchConfiguration('mouse'))
+    )
+
    container = ComposableNodeContainer(


プロセス内通信を使用してトピックのやり取りを高速化したいです。
各ノードのuse_intra_process_commsをTrueにしてください

実装例：https://github.com/SSL-Roots/consai_ros2/blob/4b539f3fe7870fb5e510d76e920470f9efdf4132/consai_robot_controller/launch/test.launch.py#L78

参考：https://docs.ros.org/en/foxy/How-To-Guides/Launching-composable-nodes.html#launch-file-examples

ShotaAk

オッケーです！
1箇所コメントしたので修正お願いします。

ShotaAk · 2023-11-01T08:33:55Z

src/object_tracking_component.cpp

usb_camノードを使用するので、raw_imageトピックとpublisherが不要になりました。
削除お願いします。

READMEも合わせて修正お願いします。

ShotaAk

LGTMです！

* 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

YusukeKato added 3 commits October 26, 2023 17:40

画像トピックのサブスクライバを実装

ef15551

画像トピックがRGBなのでBGRに変換

a46a50c

mouseとcamera_nodeのオンオフを実装

97cff9b

YusukeKato added the Type: Feature New Feature label Oct 27, 2023

YusukeKato requested a review from ShotaAk October 27, 2023 04:39

YusukeKato self-assigned this Oct 27, 2023

実機で動作するように変更

8d62f59

YusukeKato marked this pull request as ready for review October 30, 2023 07:17

物体追従のパラメータ調整

b267120

ShotaAk suggested changes Oct 31, 2023

View reviewed changes

YusukeKato added 8 commits November 1, 2023 15:04

不要なメンバ変数を削除

29fb1f1

コールバック関数の名前を変更

a90a9a6

コメントアウトしていた不要な行を削除

247b119

rclcpp::SensorDataQoS()を使用するように変更

e82b683

std::placeholders::_1を直接書くように修正

a336ee2

raspimouseノードにComposableNodeを使用

199f043

usb_camノードにもComposableNodeを使用

572ae26

各ノードのuse_intra_process_commsをtrueに変更

208fd25

ShotaAk closed this Nov 1, 2023

ShotaAk reopened this Nov 1, 2023

ShotaAk suggested changes Nov 1, 2023

View reviewed changes

YusukeKato added 3 commits November 1, 2023 17:54

image_pub_を削除

38f8363

READMEのobject_trackingのコマンドを修正

c068c55

Gazeboの場合のコマンド例を削除

817a92c

ShotaAk approved these changes Nov 1, 2023

View reviewed changes

ShotaAk merged commit 7c323ab into master Nov 1, 2023
1 check passed

ShotaAk deleted the subscribe-image-topic branch November 1, 2023 09:29

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

object_trackingにおいて画像トピックをサブスクライブするように変更 #43

object_trackingにおいて画像トピックをサブスクライブするように変更 #43

YusukeKato commented Oct 27, 2023 •

edited

Loading

ShotaAk left a comment

ShotaAk Oct 31, 2023

ShotaAk Oct 31, 2023

ShotaAk Oct 31, 2023

ShotaAk Oct 31, 2023

ShotaAk Oct 31, 2023

ShotaAk Oct 31, 2023

ShotaAk Oct 31, 2023

ShotaAk Oct 31, 2023

ShotaAk Oct 31, 2023

ShotaAk left a comment

ShotaAk Nov 1, 2023

ShotaAk left a comment

object_trackingにおいて画像トピックをサブスクライブするように変更 #43

object_trackingにおいて画像トピックをサブスクライブするように変更 #43

Conversation

YusukeKato commented Oct 27, 2023 • edited Loading

What does this implement/fix?

Does this close any currently open issues?

How has this been tested?

シミュレータの場合

実機の場合

Any other comments?

Checklists

ShotaAk left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ShotaAk left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ShotaAk left a comment

Choose a reason for hiding this comment

YusukeKato commented Oct 27, 2023 •

edited

Loading