RGBカメラによるライントレースの実装 #47

YusukeKato · 2023-11-22T07:45:37Z

What does this implement/fix?

ラズパイマウスに搭載したRGBカメラを使用してライントレースを行うサンプルを実装しました。

Does this close any currently open issues?

しません。

How has this been tested?

シミュレータ環境で動作確認

端末1で下記のコマンドを実行して、シミュレータを起動

ros2 launch raspimouse_gazebo raspimouse_with_line_follower_field.launch.py use_rgb_camera:=true camera_downward:=true

端末2で下記のコマンドを実行して、RGBカメラによるライントレースのノードを起動

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

端末3で下記のコマンドを実行して、switchesトピックを介してcmd_velトピックの配信

ros2 topic pub --once /switches raspimouse_msgs/msg/Switches "{switch0: false, switch1: false, switch2: true}"

止める場合は下記のコマンドを実行

ros2 topic pub --once /switches raspimouse_msgs/msg/Switches "{switch0: true, switch1: false, switch2: false}"

実機で動作確認

下記のコマンドを実行

ros2 launch raspimouse_ros2_examples camera_line_follower.launch.py

スイッチ2を押すと走り始めて、スイッチ0を押すと止まります。

Any other comments?

シミュレータで動作確認する場合、下記のリポジトリが必要です。

raspimouse_descriptionのros2ブランチ

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

raspimouse_simのros2ブランチ

https://github.com/rt-net/raspimouse_sim/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 · 2023-11-28T01:52:47Z

オドメトリがずれますね。raspimouse_simで呼び出しているコントローラのパラメータを見直して、改善するか確認してほしいです。

（自己位置補正がないので限界はあります）

ShotaAk

動作確認できました。コードレビューは途中です。

一旦コメントの確認と修正お願いします。

ShotaAk · 2023-11-28T01:55:22Z

include/raspimouse_ros2_examples/camera_line_follower_component.hpp

@@ -0,0 +1,82 @@
+// Copyright 2020 RT Corporation


Suggested change

// Copyright 2020 RT Corporation

// Copyright 2023 RT Corporation

ソースファイルも同様に対応してください。

ShotaAk · 2023-11-28T01:55:31Z

launch/camera_line_follower.launch.py

@@ -0,0 +1,90 @@
+# Copyright 2020 RT Corporation


Suggested change

# Copyright 2020 RT Corporation

# Copyright 2023 RT Corporation

ShotaAk · 2023-11-28T01:59:32Z

src/camera_line_follower_component.cpp

@@ -0,0 +1,272 @@
+// Copyright 2020 RT Corporation


Suggested change

// Copyright 2020 RT Corporation

// Copyright 2023 RT Corporation

ShotaAk · 2023-11-28T02:01:03Z

src/camera_line_follower_component.cpp

+using namespace std::chrono_literals;
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;


他コードへの影響を抑えるため、usingはnamespace内で使用してください。

namespace hoge { using fuga = piyo; }

ShotaAk · 2023-11-28T02:03:34Z

src/camera_line_follower_component.cpp

+void Camera_Follower::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<sensor_msgs::msg::Image>();


msgは使われてないです。

Suggested change

auto msg = std::make_unique<sensor_msgs::msg::Image>();

ShotaAk · 2023-11-28T02:18:52Z

src/camera_line_follower_component.cpp

+  const double LINEAR_VEL = 0.2;  // unit: m/s
+  const double ANGULAR_VEL = -0.8;  // unit: rad/s
+  const double TARGET_AREA = 0.1;  // 0.0 ~ 1.0
+  const double OBJECT_AREA_THRESHOLD = 0.01;  // 0.0 ~ 1.0


コンパイル時に定数化できるのでconstexprを使いましょう。

Suggested change

const double LINEAR_VEL = 0.2; // unit: m/s

const double ANGULAR_VEL = -0.8; // unit: rad/s

const double TARGET_AREA = 0.1; // 0.0 ~ 1.0

const double OBJECT_AREA_THRESHOLD = 0.01; // 0.0 ~ 1.0

constexpr double LINEAR_VEL = 0.2; // 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

ShotaAk · 2023-11-28T02:19:39Z

src/camera_line_follower_component.cpp

+  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


ここではオブジェクトをdetectしてないので文章を変えてください。

detectされたobjectを追うように速度を出す　ならわかります。

ShotaAk · 2023-11-28T02:21:40Z

src/camera_line_follower_component.cpp

+  if (switches_.switch0) {
+    RCLCPP_INFO(this->get_logger(), "Stop following.");
+    can_publish_cmdvel_ = false;
+  } else if (switches_.switch2) {
+    RCLCPP_INFO(this->get_logger(), "Start following.");
+    can_publish_cmdvel_ = true;
+  }


ここの処理はswitchesのcallback関数に移動したほうが分かりやすいです。

そうするとswitches_変数が不要になります。

ShotaAk · 2023-11-28T02:26:24Z

src/camera_line_follower_component.cpp

+  }
+  switches_ = raspimouse_msgs::msg::Switches();  // Reset switch values
+
+  if (!can_publish_cmdvel_) {


can_publish_cmdvel_がfalseでもcmd_velをpublishしているので、変数名と処理が一致してません。

enable_following_ のような変数名はどうでしょうか？

ありがとうございます。
enable_following_に変更しました。

ShotaAk · 2023-11-28T02:28:18Z

src/camera_line_follower_component.cpp

+  msg.header.frame_id = "camera_frame";
+}
+
+void Camera_Follower::following(const cv::Mat & input_frame, cv::Mat & result_frame)


ここでは追跡ではなく、追跡するための物体検出をしています。
関数名を変更してください。

detecting_line()に変更しました。

ShotaAk

その他のコードを読みました。コメントの確認おねがします。

ShotaAk · 2023-11-28T08:52:48Z

src/camera_line_follower_component.cpp

+    cv::putText(
+      result_frame, text, cv::Point(0, 30),
+      cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 0, 0), 2);


テキストが見にくいので明るい色に変えてください。

青から白に変更しました。

ShotaAk · 2023-11-28T08:56:44Z

src/camera_line_follower_component.cpp

+    );
+    // Normalize the the contour area to [0.0, 1.0].
+    object_normalized_area_ = max_area / (input_frame.rows * input_frame.cols);
+    object_is_detected_ = true;


object_is_detected_ の変更箇所をわかりやすくするため、
この関数の返り値をbool型に変更してください

例（image_callback()内での処理）：

// following関数名は適宜変更してください。 object_is_detected_ = following();

この変更に伴い、following関数をconstメンバ関数にしてください。

bool following() const;

この関数にconstを付けると

object_normalized_area_ = max_area / (input_frame.rows * input_frame.cols);

の行でエラーが発生します。

変数が２つありましたね、変更なしでOKです。

ShotaAk · 2023-11-28T09:03:27Z

src/camera_line_follower_component.cpp

+      "Service motor_power is not avaliable.");
+    return CallbackReturn::FAILURE;
+  }
+  auto request = std::make_shared<std_srvs::srv::SetBool::Request>();


サービスが失敗したらFAILUREするように実装してくれると嬉しいです。

「サービスが失敗したらFAILUREする」とは具体的にはどういう処理を指すでしょうか。
単純にプログラムを終了すれば良いでしょうか。

motor_power サービスが失敗する可能性もあるので、そのときはon_activateが失敗するように実装お願いします。

参考情報：https://robotics.stackexchange.com/a/96076/37280

auto future_result = motor_power_client_->async_send_request(request); if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) != rclcpp::executor::FutureReturnCode::SUCCESS) { return CallbackReturn::FAILURE; }

ただ、spin周りはROS 2仕様による不具合があるので、エラーが出る場合はあきらめましょう。
ros2/rclcpp#1916

Co-authored-by: Shota Aoki <[email protected]>

YusukeKato · 2023-12-01T07:43:32Z

オドメトリがずれますね。raspimouse_simで呼び出しているコントローラのパラメータを見直して、改善するか確認してほしいです。

（自己位置補正がないので限界はあります）

こちらのraspimouse_simのPRでオドメトリのズレに対応しました。
rt-net/raspimouse_sim#76

Co-authored-by: Shota Aoki <[email protected]>

ShotaAk · 2023-12-06T02:41:48Z

オドメトリの調整いい感じです

ShotaAk · 2023-12-06T02:49:19Z

src/camera_line_follower_component.cpp

+  // Detect and follow the line
  // when the number of pixels of the object is greater than the threshold.


ここではDetectしてないので訂正してほしいです。（画像処理もここで行われている　と勘違いします）

Suggested change

// Detect and follow the line

// when the number of pixels of the object is greater than the threshold.

// Follow the line

// when the number of pixels of the object is greater than the threshold.

ShotaAk · 2023-12-06T02:56:33Z

src/camera_line_follower_component.cpp

+    );
+    // Normalize the the contour area to [0.0, 1.0].
+    object_normalized_area_ = max_area / (input_frame.rows * input_frame.cols);
+    object_is_detected_ = true;


変数が２つありましたね、変更なしでOKです。

ShotaAk

細かいところにコメントしました。確認お願いします

ShotaAk · 2023-12-06T05:00:45Z

src/camera_line_follower_component.cpp

+      "Service motor_power is not avaliable.");
+    return CallbackReturn::FAILURE;
+  }
+  auto request = std::make_shared<std_srvs::srv::SetBool::Request>();


motor_power サービスが失敗する可能性もあるので、そのときはon_activateが失敗するように実装お願いします。

参考情報：https://robotics.stackexchange.com/a/96076/37280

auto future_result = motor_power_client_->async_send_request(request); if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) != rclcpp::executor::FutureReturnCode::SUCCESS) { return CallbackReturn::FAILURE; }

ただ、spin周りはROS 2仕様による不具合があるので、エラーが出る場合はあきらめましょう。
ros2/rclcpp#1916

ShotaAk · 2023-12-06T05:07:36Z

src/camera_line_follower_component.cpp

+void Camera_Follower::convert_frame_to_message(
+  const cv::Mat & frame, sensor_msgs::msg::Image & msg)
+{


これもconstメンバ関数にできそうです

Co-authored-by: Shota Aoki <[email protected]>

YusukeKato · 2023-12-06T09:13:28Z

下記のコードを追加したところエラーが発生し、ライントレースは正常に動きませんでした。

auto future_result = motor_power_client_->async_send_request(request);
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future_result) 
    != rclcpp::FutureReturnCode::SUCCESS)
{
  return CallbackReturn::FAILURE;
}

エラー内容↓

[component_container_mt-1] [ERROR] [1701853265.381801467] [camera_follower]: Caught exception in callback for transition 13
[component_container_mt-1] [ERROR] [1701853265.381822847] [camera_follower]: Original error: Node '/camera_follower' has already been added to an executor.
[component_container_mt-1] [WARN] [1701853265.381833738] [camera_follower]: Error occurred while doing error handling.
[lifecycle_node_manager-2] [ERROR] [1701853265.382155052] [manager]: Failed to activate nodes.
[component_container_mt-1] [WARN] [1701853265.452359650] [LifecyclePublisher]: Trying to publish message on the topic '/result_image', but the publisher is not activated

ShotaAk · 2023-12-06T09:26:56Z

エラーについて了解です。ノード外部にexecutorがいるのでspinの権限を取られているんですよね。。。

ひとまずこのままにしましょう

ShotaAk

OKです！マージします

YusukeKato added 4 commits November 21, 2023 18:21

カメラによるライントレース用のファイルを作成

004eac5

実行可能な状態に更新

31c1e2a

ライントレースが実行可能な状態に更新

d524acc

スイッチ0を押してcmd_velの配信をオンオフできるように変更

a607ff5

YusukeKato requested a review from ShotaAk November 22, 2023 07:56

YusukeKato self-assigned this Nov 22, 2023

YusukeKato added the Type: Feature New Feature label Nov 22, 2023

YusukeKato added 2 commits November 22, 2023 18:01

スイッチの処理を修正

b72a1c9

colcon testが通るように修正

a9ad829

YusukeKato marked this pull request as ready for review November 22, 2023 09:32

ShotaAk suggested changes Nov 28, 2023

View reviewed changes

YusukeKato and others added 3 commits December 1, 2023 16:15

Copyrightの文面を修正

125abf2

Co-authored-by: Shota Aoki <[email protected]>

Copyrightの文面を修正

1dad0d4

Co-authored-by: Shota Aoki <[email protected]>

Copyrightの文面を修正

1fb0729

Co-authored-by: Shota Aoki <[email protected]>

YusukeKato mentioned this pull request Dec 1, 2023

コントローラのパラメータを調整してオドメトリのズレを修正 rt-net/raspimouse_sim#76

Merged

2 tasks

YusukeKato and others added 12 commits December 5, 2023 14:19

使用されていないmsgを削除

52f33a5

Co-authored-by: Shota Aoki <[email protected]>

usingをnamespace内で使用するように変更

02ed75e

変数にconstを付ける

d8099b5

is_bigendianの行を削除

349b400

内部変数を変更しない関数にconstを付ける

4969c4b

Co-authored-by: Shota Aoki <[email protected]>

ソースコードのほうでもconstを付ける

442878b

cmd_velを関数内で宣言する

d8891c0

constをconstexprに変更

445f6db

Co-authored-by: Shota Aoki <[email protected]>

コメントを修正

e4d4f16

スイッチの処理を修正

48c00f5

following()をdetecting_lineに変更

a1a8154

検出結果の文字の色を青から白に変更

b1b3ceb

ShotaAk reviewed Dec 6, 2023

View reviewed changes

ShotaAk suggested changes Dec 6, 2023

View reviewed changes

YusukeKato and others added 3 commits December 6, 2023 15:31

コメントを修正

8f559e8

Co-authored-by: Shota Aoki <[email protected]>

返り値をbool型に変更

f6067af

関数にconstを付ける

5a17a7d

ShotaAk approved these changes Dec 6, 2023

View reviewed changes

ShotaAk merged commit 153c25c into master Dec 6, 2023
2 checks passed

ShotaAk deleted the add-camera-line-follower branch December 6, 2023 09:36

ShotaAk mentioned this pull request Jan 12, 2024

Add usb_cam dependency #48

Merged

2 tasks

	// Copyright 2020 RT Corporation
	// Copyright 2023 RT Corporation

	# Copyright 2020 RT Corporation
	# Copyright 2023 RT Corporation

		using namespace std::chrono_literals;
		using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

		// Detect and follow the line
		// when the number of pixels of the object is greater than the threshold.

RGBカメラによるライントレースの実装 #47

RGBカメラによるライントレースの実装 #47

Conversation

YusukeKato commented Nov 22, 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 commented Nov 28, 2023

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

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

YusukeKato Dec 5, 2023 • edited Loading

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

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

YusukeKato commented Dec 1, 2023

ShotaAk commented Dec 6, 2023

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

Choose a reason for hiding this comment

YusukeKato commented Dec 6, 2023

ShotaAk commented Dec 6, 2023

ShotaAk left a comment

Choose a reason for hiding this comment

YusukeKato commented Nov 22, 2023 •

edited

Loading

YusukeKato Dec 5, 2023 •

edited

Loading