diff --git a/README.en.md b/README.en.md
index e586116..95676a6 100644
--- 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
+
+
+
+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.**
+
+
+
+#### 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
index 2b82003..93052cb 100644
--- 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
+
+
+
+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や画像トピックが発行されないことがあります。**
+
+
+
+#### 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
diff --git a/include/raspimouse_ros2_examples/camera_line_follower_component.hpp b/include/raspimouse_ros2_examples/camera_line_follower_component.hpp
index 0e7a756..262cacd 100644
--- 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
index f43c09f..fd933d5 100644
--- 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
index 08abcc9..2c316bb 100644
--- 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(),
+ get_parameter(BRIGHTNESS_MAX_VAL_PARAM).get_value(),
+ 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> contours;
std::vector 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(
"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;
}