カメラライントレースを修正

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,58 @@ 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 />
+
+#### Configure
+
+Edit [`./src/camera_line_follower_component.cpp`](./src/camera_line_follower_component.cpp)
+to change a color of tracking target.
+
+If the object detection accuracy is poor, adjust the camera exposure and parameters in the function
+
+```cpp
+cv::inRange(gray, 0, 100, extracted_bin);
+```
+
+[back to example list](#how-to-use-examples)
+
 --- 
 
 ### SLAM

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,59 @@ 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 />
+
+#### Configure
+
+追跡対象の色を変更するには
+[`./src/camera_line_follower_component.cpp`](./src/camera_line_follower_component.cpp)
+を編集します。
+
+物体検出精度が悪い時にはカメラの露光や関数内のパラメータを調整して下さい。
+
+```cpp
+cv::inRange(gray, 0, 100, extracted_bin);
+```
+
+[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 />

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'))),

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
 {
@@ -68,17 +71,16 @@ 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.angular.z = ANGULAR_VEL * object_normalized_point_.x;
+    cmd_vel.linear.x = LINEAR_VEL;
+    cmd_vel.angular.z = ANGULAR_VEL * object_normalized_point_.x * 2.0;
   } else {
     cmd_vel.linear.x = 0.0;
     cmd_vel.angular.z = 0.0;
@@ -131,7 +133,11 @@ bool Camera_Follower::detecting_line(const cv::Mat & input_frame, cv::Mat & resu
   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, 100);
+
   return CallbackReturn::SUCCESS;
 }