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

include/raspimouse_ros2_examples/camera_line_follower_component.hpp

@@ -1,4 +1,4 @@
-// Copyright 2020 RT Corporation
+// Copyright 2023 RT Corporation
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -46,24 +46,22 @@ class Camera_Follower : public rclcpp_lifecycle::LifecycleNode
 private:
   cv::VideoCapture cap_;
   bool object_is_detected_;
-  bool can_publish_cmdvel_;
+  bool enable_following_;
   cv::Point2d object_normalized_point_;
   double object_normalized_area_;
-  geometry_msgs::msg::Twist cmd_vel_;
-  raspimouse_msgs::msg::Switches switches_;
   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::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
   rclcpp::Subscription<raspimouse_msgs::msg::Switches>::SharedPtr switches_sub_;
   rclcpp::TimerBase::SharedPtr cmd_vel_timer_;
 
-  std::string mat_type2encoding(int mat_type);
+  std::string mat_type2encoding(const int mat_type) const;
   void convert_frame_to_message(
     const cv::Mat & frame,
     sensor_msgs::msg::Image & msg);
 
-  void following(const cv::Mat & input_frame, cv::Mat & result_frame);
+  void detecting_line(const cv::Mat & input_frame, cv::Mat & result_frame);
 
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
   on_configure(const rclcpp_lifecycle::State &);

launch/camera_line_follower.launch.py

@@ -1,4 +1,4 @@
-# Copyright 2020 RT Corporation
+# Copyright 2023 RT Corporation
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.

src/camera_line_follower_component.cpp

@@ -1,4 +1,4 @@
-// Copyright 2020 RT Corporation
+// Copyright 2023 RT Corporation
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -24,14 +24,15 @@
 #include <opencv2/opencv.hpp>
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
+#include "geometry_msgs/msg/twist.hpp"
 #include "lifecycle_msgs/srv/change_state.hpp"
 #include "cv_bridge/cv_bridge.h"
 
-using namespace std::chrono_literals;
-using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
 namespace camera_line_follower
 {
+using namespace std::chrono_literals;
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
 Camera_Follower::Camera_Follower(const rclcpp::NodeOptions & options)
 : rclcpp_lifecycle::LifecycleNode("camera_follower", options),
@@ -41,64 +42,59 @@ Camera_Follower::Camera_Follower(const rclcpp::NodeOptions & options)
 
 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>();
+  const auto cv_img = cv_bridge::toCvShare(msg_image, msg_image->encoding);
   auto result_msg = std::make_unique<sensor_msgs::msg::Image>();
-  msg->is_bigendian = false;
-  result_msg->is_bigendian = false;
 
   cv::Mat frame, result_frame;
   cv::cvtColor(cv_img->image, frame, CV_RGB2BGR);
 
   if (!frame.empty()) {
-    following(frame, result_frame);
+    detecting_line(frame, result_frame);
     convert_frame_to_message(result_frame, *result_msg);
     result_image_pub_->publish(std::move(result_msg));
   }
 }
 
 void Camera_Follower::callback_switches(const raspimouse_msgs::msg::Switches::SharedPtr msg)
 {
-  switches_ = *msg;
+  if (msg->switch0) {
+    RCLCPP_INFO(this->get_logger(), "Stop following.");
+    enable_following_ = false;
+  } else if (msg->switch2) {
+    RCLCPP_INFO(this->get_logger(), "Start following.");
+    enable_following_ = true;
+  }
 }
 
 void Camera_Follower::on_cmd_vel_timer()
 {
-  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
+  geometry_msgs::msg::Twist cmd_vel;
 
-  // Detects an object and tracks it
+  // Detect and 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 * (object_normalized_area_ - TARGET_AREA);
+    cmd_vel.angular.z = ANGULAR_VEL * object_normalized_point_.x;
   } else {
-    cmd_vel_.linear.x = 0.0;
-    cmd_vel_.angular.z = 0.0;
-  }
-
-  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;
+    cmd_vel.linear.x = 0.0;
+    cmd_vel.angular.z = 0.0;
   }
-  switches_ = raspimouse_msgs::msg::Switches();  // Reset switch values
 
-  if (!can_publish_cmdvel_) {
-    cmd_vel_.linear.x = 0.0;
-    cmd_vel_.angular.z = 0.0;
+  if (!enable_following_) {
+    cmd_vel.linear.x = 0.0;
+    cmd_vel.angular.z = 0.0;
   }
 
-  auto msg = std::make_unique<geometry_msgs::msg::Twist>(cmd_vel_);
+  auto msg = std::make_unique<geometry_msgs::msg::Twist>(cmd_vel);
   cmd_vel_pub_->publish(std::move(msg));
 }
 
 // Ref: https://github.com/ros2/demos/blob/dashing/image_tools/src/cam2image.cpp
-std::string Camera_Follower::mat_type2encoding(int mat_type)
+std::string Camera_Follower::mat_type2encoding(const int mat_type) const
 {
   switch (mat_type) {
     case CV_8UC1:
@@ -129,7 +125,7 @@ void Camera_Follower::convert_frame_to_message(
   msg.header.frame_id = "camera_frame";
 }
 
-void Camera_Follower::following(const cv::Mat & input_frame, cv::Mat & result_frame)
+void Camera_Follower::detecting_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;
@@ -179,7 +175,7 @@ void Camera_Follower::following(const cv::Mat & input_frame, cv::Mat & result_fr
     cv::circle(result_frame, mt_point, 30, cv::Scalar(0, 0, 255), 2, cv::LINE_4);
     cv::putText(
       result_frame, text, cv::Point(0, 30),
-      cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 0, 0), 2);
+      cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255), 2);
   } else {
     object_is_detected_ = false;
   }
@@ -234,7 +230,6 @@ CallbackReturn Camera_Follower::on_deactivate(const rclcpp_lifecycle::State &)
   cmd_vel_timer_->cancel();
 
   object_is_detected_ = false;
-  cmd_vel_ = geometry_msgs::msg::Twist();
 
   return CallbackReturn::SUCCESS;
 }