カメラライントレースを修正 (#49)

* READMEにカメラライントレースの情報を追記 * リサイズした画像に変更 * 目次にカメラライントレースの項目を追加 * rqt_image_viewのリンクが切れていたので更新 * usb_camの配信頻度を30FPSから10FPSに変更 * READMEにカメラライントレースのスイッチ操作の説明を追加 * usb_camの画像サイズを320x240に設定 * adaptiveThreshold関数で画像を2値化 * 速度調整&inRange関数使用&findContours関数の引数を変更 * 2値化のしきい値をパラメータで設定 * デバッグ用の出力を削除 * コードの体裁を整える * READMEに2値化のしきい値のパラメータの設定方法を追加 * デバッグ用に使用していた係数を削除 * detecting_line()をdetect_line()に変更 * READMEのConfigureの項目を削除 * シミュレータ環境でライントレースが行えるように2値化のしきい値を調整
rt-net · Feb 5, 2024 · 85b3bf0 · 85b3bf0
1 parent 8eedd49
commit 85b3bf0
Show file tree

Hide file tree

Showing 5 changed files with 139 additions and 11 deletions.
diff --git a/README.en.md b/README.en.md
@@ -61,6 +61,7 @@ This repository is licensed under the Apache 2.0, see [LICENSE](./LICENSE) for d
 - [joystick_control](#joystick_control)
 - [object_tracking](#object_tracking)
 - [line_follower](#line_follower)
+- [camera_line_follower](#camera_line_follower)
 - [SLAM](#slam)
 - [direction_controller](#direction_controller)
 
@@ -162,7 +163,7 @@ $ ros2 launch raspimouse_ros2_examples object_tracking.launch.py video_device:=/
 
 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/).
+or [rqt_image_view](https://index.ros.org/p/rqt_image_view/).
 
 **Viewing an image may cause the node to behave unstable and not publish cmd_vel or image topics.**
 
@@ -247,6 +248,62 @@ void Follower::publish_cmdvel_for_line_following(void)
 
 [back to example list](#how-to-use-examples)
 
+---
+
+### camera_line_follower
+
+<img src=https://rt-net.github.io/images/raspberry-pi-mouse/mouse_camera_line_trace_2.png width=500 />
+
+This is an example for line following by RGB camera.
+
+#### Requirements 
+
+- Web camera
+  - [Logicool HD WEBCAM C310N](https://www.logicool.co.jp/ja-jp/product/hd-webcam-c310n)
+- Camera mount
+  - [Raspberry Pi Mouse Option kit No.4 \[Webcam mount\]](https://www.rt-shop.jp/index.php?main_page=product_info&cPath=1299_1395&products_id=3584&language=en)
+
+#### Installation
+
+Install a camera mount and a web camera to Raspberry Pi Mouse, then connect the camera to the Raspberry Pi．
+
+#### How to use
+
+Then, launch nodes with the following command:
+
+```sh
+$ ros2 launch raspimouse_ros2_examples camera_line_follower.launch.py video_device:=/dev/video0
+```
+
+Place Raspberry Pi Mouse on the line and press SW2 to start line following.
+
+Press SW0 to stop the following.
+
+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/p/rqt_image_view/).
+
+**Viewing an image may cause the node to behave unstable and not publish cmd_vel or image topics.**
+
+<img src=https://rt-net.github.io/images/raspberry-pi-mouse/camera_line_trace.png width=500 />
+
+#### Parameters
+
+- `brightness_max_value`
+  - Type: `int`
+  - Default: 90
+  - Maximum threshold value for image binarisation.
+- `brightness_min_value`
+  - Type: `int`
+  - Default: 0
+  - Minimum threshold value for image binarisation.
+
+```sh
+ros2 param set /camera_follower brightness_max_value 80
+```
+
+[back to example list](#how-to-use-examples)
+
 --- 
 
 ### SLAM

diff --git a/README.md b/README.md
@@ -62,6 +62,7 @@ $ source ~/ros2_ws/install/setup.bash
 - [joystick_control](#joystick_control)
 - [object_tracking](#object_tracking)
 - [line_follower](#line_follower)
+- [camera_line_follower](#camera_line_follower)
 - [SLAM](#slam)
 - [direction_controller](#direction_controller)
 
@@ -164,7 +165,7 @@ $ ros2 launch raspimouse_ros2_examples object_tracking.launch.py video_device:=/
 
 カメラ画像は`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/)
+や[rqt_image_view](https://index.ros.org/p/rqt_image_view/)
 で表示できます。
 
 **画像を表示するとノードの動作が不安定になり、cmd_velや画像トピックが発行されないことがあります。**
@@ -253,6 +254,62 @@ void Follower::publish_cmdvel_for_line_following(void)
 
 --- 
 
+### camera_line_follower
+
+<img src=https://rt-net.github.io/images/raspberry-pi-mouse/mouse_camera_line_trace_2.png width=500 />
+
+RGBカメラによるライントレースのコード例です。
+
+#### Requirements 
+
+- Webカメラ
+  - [Logicool HD WEBCAM C310N](https://www.logicool.co.jp/ja-jp/product/hd-webcam-c310n)
+- カメラマウント
+  - [Raspberry Pi Mouse オプションキット No.4 \[Webカメラマウント\]](https://www.rt-shop.jp/index.php?main_page=product_info&cPath=1299_1395&products_id=3584)
+
+#### Installation
+
+Raspberry Pi Mouseにカメラマウントを取り付け、WebカメラをRaspberry Piに接続します。
+
+#### How to use
+
+次のコマンドでノードを起動します。
+
+```sh
+$ ros2 launch raspimouse_ros2_examples camera_line_follower.launch.py video_device:=/dev/video0
+```
+
+ライン上にRaspberry Pi Mouseを置き、SW2を押してライントレースを開始します。
+停止させる場合はSW0を押します。
+
+カメラ画像は`camera/color/image_raw`、物体検出画像は`result_image`というトピックとして発行されます。
+これらの画像は[RViz](https://index.ros.org/r/rviz/)
+や[rqt_image_view](https://index.ros.org/p/rqt_image_view/)
+で表示できます。
+
+**画像を表示するとノードの動作が不安定になり、cmd_velや画像トピックが発行されないことがあります。**
+
+<img src=https://rt-net.github.io/images/raspberry-pi-mouse/camera_line_trace.png width=500 />
+
+#### Parameters
+
+- `brightness_max_value`
+  - Type: `int`
+  - Default: 90
+  - 画像の2値化のしきい値の最大値
+- `brightness_min_value`
+  - Type: `int`
+  - Default: 0
+  - 画像の2値化のしきい値の最小値
+
+```sh
+ros2 param set /camera_follower brightness_max_value 80
+```
+
+[back to example list](#how-to-use-examples)
+
+--- 
+
 ### SLAM
 
 <img src=https://rt-net.github.io/images/raspberry-pi-mouse/slam_toolbox_ros2.png width=500 />

diff --git a/include/raspimouse_ros2_examples/camera_line_follower_component.hpp b/include/raspimouse_ros2_examples/camera_line_follower_component.hpp
@@ -61,7 +61,7 @@ class Camera_Follower : public rclcpp_lifecycle::LifecycleNode
     const cv::Mat & frame,
     sensor_msgs::msg::Image & msg) const;
 
-  bool detecting_line(const cv::Mat & input_frame, cv::Mat & result_frame);
+  bool detect_line(const cv::Mat & input_frame, cv::Mat & result_frame);
 
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
   on_configure(const rclcpp_lifecycle::State &);

diff --git a/launch/camera_line_follower.launch.py b/launch/camera_line_follower.launch.py
@@ -65,7 +65,11 @@ def generate_launch_description():
                     parameters=[
                         {'video_device': LaunchConfiguration('video_device')},
                         {'frame_id': 'camera_color_optical_frame'},
-                        {'pixel_format': 'yuyv2rgb'}
+                        {'pixel_format': 'yuyv2rgb'},
+                        {'image_width': 320},
+                        {'image_height': 240},
+                        {'auto_white_balance': False},
+                        {'autoexposure': False}
                     ],
                     extra_arguments=[{'use_intra_process_comms': True}],
                     condition=IfCondition(LaunchConfiguration('use_camera_node'))),

diff --git a/src/camera_line_follower_component.cpp b/src/camera_line_follower_component.cpp
@@ -28,6 +28,9 @@
 #include "lifecycle_msgs/srv/change_state.hpp"
 #include "cv_bridge/cv_bridge.h"
 
+constexpr auto BRIGHTNESS_MIN_VAL_PARAM = "brightness_min_value";
+constexpr auto BRIGHTNESS_MAX_VAL_PARAM = "brightness_max_value";
+
 
 namespace camera_line_follower
 {
@@ -49,7 +52,7 @@ void Camera_Follower::image_callback(const sensor_msgs::msg::Image::SharedPtr ms
   cv::cvtColor(cv_img->image, frame, CV_RGB2BGR);
 
   if (!frame.empty()) {
-    object_is_detected_ = detecting_line(frame, result_frame);
+    object_is_detected_ = detect_line(frame, result_frame);
     convert_frame_to_message(result_frame, *result_msg);
     result_image_pub_->publish(std::move(result_msg));
   }
@@ -68,16 +71,15 @@ void Camera_Follower::callback_switches(const raspimouse_msgs::msg::Switches::Sh
 
 void Camera_Follower::on_cmd_vel_timer()
 {
-  constexpr double LINEAR_VEL = 0.2;  // unit: m/s
+  constexpr double LINEAR_VEL = 0.05;  // unit: m/s
   constexpr double ANGULAR_VEL = -0.8;  // unit: rad/s
-  constexpr double TARGET_AREA = 0.1;  // 0.0 ~ 1.0
   constexpr double OBJECT_AREA_THRESHOLD = 0.01;  // 0.0 ~ 1.0
   geometry_msgs::msg::Twist cmd_vel;
 
   // Follow the line
   // when the number of pixels of the object is greater than the threshold.
   if (object_is_detected_ && object_normalized_area_ > OBJECT_AREA_THRESHOLD) {
-    cmd_vel.linear.x = LINEAR_VEL * (object_normalized_area_ - TARGET_AREA);
+    cmd_vel.linear.x = LINEAR_VEL;
     cmd_vel.angular.z = ANGULAR_VEL * object_normalized_point_.x;
   } else {
     cmd_vel.linear.x = 0.0;
@@ -125,13 +127,17 @@ void Camera_Follower::convert_frame_to_message(
   msg.header.frame_id = "camera_frame";
 }
 
-bool Camera_Follower::detecting_line(const cv::Mat & input_frame, cv::Mat & result_frame)
+bool Camera_Follower::detect_line(const cv::Mat & input_frame, cv::Mat & result_frame)
 {
   // Specific colors are extracted from the input image and converted to binary values.
   cv::Mat gray;
   cv::cvtColor(input_frame, gray, cv::COLOR_BGR2GRAY);
   cv::Mat extracted_bin;
-  cv::inRange(gray, 0, 100, extracted_bin);
+  cv::inRange(
+    gray,
+    get_parameter(BRIGHTNESS_MIN_VAL_PARAM).get_value<int>(),
+    get_parameter(BRIGHTNESS_MAX_VAL_PARAM).get_value<int>(),
+    extracted_bin);
   input_frame.copyTo(result_frame, extracted_bin);
 
   // Remove noise with morphology transformation
@@ -141,7 +147,7 @@ bool Camera_Follower::detecting_line(const cv::Mat & input_frame, cv::Mat & resu
   // Extracting contours
   std::vector<std::vector<cv::Point>> contours;
   std::vector<cv::Vec4i> hierarchy;
-  cv::findContours(morph_bin, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
+  cv::findContours(morph_bin, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
 
   // Extracting the largest contours
   double max_area = 0;
@@ -197,6 +203,10 @@ CallbackReturn Camera_Follower::on_configure(const rclcpp_lifecycle::State &)
   switches_sub_ = create_subscription<raspimouse_msgs::msg::Switches>(
     "switches", 1, std::bind(&Camera_Follower::callback_switches, this, std::placeholders::_1));
 
+  // Set parameter defaults
+  declare_parameter(BRIGHTNESS_MIN_VAL_PARAM, 0);
+  declare_parameter(BRIGHTNESS_MAX_VAL_PARAM, 90);
+
   return CallbackReturn::SUCCESS;
 }