diff --git a/CHANGELOG.rst b/CHANGELOG.rst index ea7813e..6adf1be 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package raspimouse_ros2_examples ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-03-05) +------------------ +* READMEにSLAM&Navigationパッケージの案内を追加 (`#53 `_) +* Camera_FollowerクラスをCameraFollowerに変更 (`#52 `_) +* Update camera line follower: Set motor power with switch input. Add area_threthold param. (`#51 `_) +* Add velocity parameters for camera_line_follower (`#50 `_) +* カメラライントレースを修正 (`#49 `_) +* Change threthold of line detection +* Add usb_cam dependency (`#48 `_) +* RGBカメラによるライントレースの実装 (`#47 `_) +* リリースのためにCHANGELOG.rstとpackage.xmlを更新 (`#45 `_) +* Contributors: Shota Aoki, ShotaAk, YusukeKato + 2.1.0 (2023-11-07) ------------------ * READMEにGazeboでも実行できることを追記 (`#44 `_) diff --git a/CMakeLists.txt b/CMakeLists.txt index c6238bb..15c821f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,6 +51,23 @@ ament_target_dependencies(object_tracking_component cv_bridge) rclcpp_components_register_nodes(object_tracking_component "object_tracking::Tracker") +add_library(camera_line_follower_component SHARED + src/camera_line_follower_component.cpp) +target_compile_definitions(camera_line_follower_component + PRIVATE "RASPIMOUSE_ROS2_EXAMPLES_BUILDING_DLL") +ament_target_dependencies(camera_line_follower_component + rclcpp + rclcpp_components + rclcpp_lifecycle + std_msgs + std_srvs + sensor_msgs + geometry_msgs + OpenCV + cv_bridge + raspimouse_msgs) +rclcpp_components_register_nodes(camera_line_follower_component "camera_line_follower::CameraFollower") + add_library(line_follower_component SHARED src/line_follower_component.cpp) target_compile_definitions(line_follower_component @@ -112,6 +129,7 @@ ament_export_dependencies(OpenCV) ament_export_include_directories(include) ament_export_libraries( object_tracking_component + camera_line_follower_component line_follower_component direction_controller_component) @@ -122,6 +140,7 @@ install( install(TARGETS object_tracking_component + camera_line_follower_component line_follower_component direction_controller_component EXPORT export_${PROJECT_NAME} diff --git a/README.en.md b/README.en.md index e586116..4934b21 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,87 +248,81 @@ void Follower::publish_cmdvel_for_line_following(void) [back to example list](#how-to-use-examples) ---- +--- -### SLAM +### camera_line_follower - + -This is an example to use LiDAR and [slam_toolbox](https://github.com/SteveMacenski/slam_toolbox) for SLAM (Simultaneous Localization And Mapping). +This is an example for line following by RGB camera. #### Requirements -- LiDAR - - - [LDS-01](https://www.rt-shop.jp/index.php?main_page=product_info&cPath=1348_5&products_id=3676&language=en) -- [LiDAR Mount](https://www.rt-shop.jp/index.php?main_page=product_info&cPath=1299_1395&products_id=3867&language=en) -- Joystick Controller (Optional) - +- 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 LiDAR to the Raspberry Pi Mouse. +Install a camera mount and a web camera to Raspberry Pi Mouse, then connect the camera to the Raspberry Pi． - -- LDS-01 - - - #### How to use -Launch nodes on Raspberry Pi Mouse with the following command: +Then, launch nodes with the following command: ```sh -# LDS -$ ros2 launch raspimouse_ros2_examples mouse_with_lidar.launch.py lidar:=lds +$ ros2 launch raspimouse_ros2_examples camera_line_follower.launch.py video_device:=/dev/video0 ``` -Next, launch `teleop_joy.launch.py` to control Raspberry Pi Mouse with the following command: - -```sh -# Use DUALSHOCK 3 -$ ros2 launch raspimouse_ros2_examples teleop_joy.launch.py joydev:="/dev/input/js0" joyconfig:=dualshock3 mouse:=false -``` +Place Raspberry Pi Mouse on the line and press SW2 to start line following. -Then, launch the slam_toolbox package (on a remote computer recommend) with the following command: +Press SW0 to stop the following. -```sh -$ ros2 launch raspimouse_ros2_examples slam.launch.py -``` - -After moving Raspberry Pi Mouse and making a map, run a node to save the map with the following command: +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/). -```sh -$ mkdir ~/maps -$ ros2 run nav2_map_server map_saver_cli -f ~/maps/mymap --ros-args -p save_map_timeout:=10000.0 -``` +**Viewing an image may cause the node to behave unstable and not publish cmd_vel or image topics.** -#### Configure SLAM parameters + -Edit [./config/mapper_params_offline.yaml](./config/mapper_params_offline.yaml) to configure parameters of [slam_toolbox](https://github.com/SteveMacenski/slam_toolbox) package. +#### Parameters -#### Configure Odometry calculation +- `max_brightness` + - Type: `int` + - Default: 90 + - Maximum threshold value for image binarisation. +- `min_brightness` + - Type: `int` + - Default: 0 + - Minimum threshold value for image binarisation. +- `max_linear_vel` + - Type: `double` + - Default: 0.05 + - Maximum linear velocity. +- `max_angular_vel` + - Type: `double` + - Default: 0.8 + - Maximum angular velocity. +- `area_threthold` + - Type: `double` + - Default: 0.20 + - Threshold value of the area of the line to start following. -Edit [mouse.yml](./config/mouse.yml) to set `use_pulse_counters` to `true` (default: `false`) then the `raspimouse` node calculate the odometry (`/odom`) from motor control pulse counts. +```sh +ros2 param set /camera_follower max_brightness 80 +``` -This improves the accuracy of self-localization. +[back to example list](#how-to-use-examples) -```yaml -raspimouse: - ros__parameters: - odometry_scale_left_wheel : 1.0 - odometry_scale_right_wheel: 1.0 - use_light_sensors : true - use_pulse_counters : true -``` +--- - +SLAM and Navigation examples for Raspberry Pi Mouse is [here](https://github.com/rt-net/raspimouse_slam_navigation_ros2). [back to example list](#how-to-use-examples) diff --git a/README.md b/README.md index 2b82003..fb8d1f4 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,89 +254,79 @@ void Follower::publish_cmdvel_for_line_following(void) --- -### SLAM +### camera_line_follower - + -LiDARと[slam_toolbox](https://github.com/SteveMacenski/slam_toolbox) -を使ってSLAM（自己位置推定と地図作成）を行うサンプルです。 +RGBカメラによるライントレースのコード例です。 #### Requirements -- LiDAR - - - [LDS-01](https://www.rt-shop.jp/index.php?main_page=product_info&cPath=1348_5&products_id=3676) -- [LiDAR Mount](https://www.rt-shop.jp/index.php?main_page=product_info&cPath=1299_1395&products_id=3867) -- Joystick Controller (Optional) - +- 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にLiDARを取り付けます。 +Raspberry Pi Mouseにカメラマウントを取り付け、WebカメラをRaspberry Piに接続します。 - -- LDS-01 - - - #### How to use -Raspberry Pi Mouse上で次のコマンドでノードを起動します。 +次のコマンドでノードを起動します。 ```sh -# LDS -$ ros2 launch raspimouse_ros2_examples mouse_with_lidar.launch.py lidar:=lds +$ ros2 launch raspimouse_ros2_examples camera_line_follower.launch.py video_device:=/dev/video0 ``` -Raspberry Pi Mouseを動かすため`teleop_joy.launch.py`を起動します +ライン上にRaspberry Pi Mouseを置き、SW2を押してライントレースを開始します。 +停止させる場合はSW0を押します。 -```sh -# Use DUALSHOCK 3 -$ ros2 launch raspimouse_ros2_examples teleop_joy.launch.py joydev:="/dev/input/js0" joyconfig:=dualshock3 mouse:=false -``` +カメラ画像は`camera/color/image_raw`、物体検出画像は`result_image`というトピックとして発行されます。 +これらの画像は[RViz](https://index.ros.org/r/rviz/) +や[rqt_image_view](https://index.ros.org/p/rqt_image_view/) +で表示できます。 -次のコマンドでslam_toolboxパッケージを起動します。（Remote computerでの実行推奨） +**画像を表示するとノードの動作が不安定になり、cmd_velや画像トピックが発行されないことがあります。** -```sh -$ ros2 launch raspimouse_ros2_examples slam.launch.py -``` + -Raspberry Pi Mouseを動かして地図を作成します。 +#### Parameters -次のコマンドで作成した地図を保存します。 +- `max_brightness` + - Type: `int` + - Default: 90 + - 画像の2値化のしきい値の最大値 +- `min_brightness` + - Type: `int` + - Default: 0 + - 画像の2値化のしきい値の最小値 +- `max_linear_vel` + - Type: `double` + - Default: 0.05 + - 直進速度の最大値 +- `max_angular_vel` + - Type: `double` + - Default: 0.8 + - 旋回速度の最大値 +- `area_threthold` + - Type: `double` + - Default: 0.20 + - 走行を開始するためのライン面積のしきい値 ```sh -$ mkdir ~/maps -$ ros2 run nav2_map_server map_saver_cli -f ~/maps/mymap --ros-args -p save_map_timeout:=10000.0 +ros2 param set /camera_follower max_brightness 80 ``` -#### Configure SLAM parameters - -[./config/mapper_params_offline.yaml](./config/mapper_params_offline.yaml)で[slam_toolbox](https://github.com/SteveMacenski/slam_toolbox)パッケージのパラメータを調節します。 - -#### Configure Odometry calculation - -下記のように[mouse.yml](./config/mouse.yml)を編集し、`use_pulse_counters`を`true`に（初期値: `false`）することで、 -`raspimouse`ノードがモータの制御パルス数からオドメトリ（`/odom`）を計算します。 - -これは自己位置推定の精度を向上させます。 +[back to example list](#how-to-use-examples) -```yaml -raspimouse: - ros__parameters: - odometry_scale_left_wheel : 1.0 - odometry_scale_right_wheel: 1.0 - use_light_sensors : true - use_pulse_counters : true -``` +--- - +Raspberry Pi MouseでSLAMとNavigationを行うサンプルは[rt-net/raspimouse_slam_navigation_ros2](https://github.com/rt-net/raspimouse_slam_navigation_ros2)へ移行しました。 [back to example list](#how-to-use-examples) diff --git a/include/raspimouse_ros2_examples/camera_line_follower_component.hpp b/include/raspimouse_ros2_examples/camera_line_follower_component.hpp new file mode 100644 index 0000000..35e89ba --- /dev/null +++ b/include/raspimouse_ros2_examples/camera_line_follower_component.hpp @@ -0,0 +1,81 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RASPIMOUSE_ROS2_EXAMPLES__CAMERA_LINE_FOLLOWER_COMPONENT_HPP_ +#define RASPIMOUSE_ROS2_EXAMPLES__CAMERA_LINE_FOLLOWER_COMPONENT_HPP_ + +#include +#include + +#include "raspimouse_msgs/msg/switches.hpp" +#include "raspimouse_ros2_examples/visibility_control.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "std_msgs/msg/string.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "opencv2/highgui/highgui.hpp" + +namespace camera_line_follower +{ + +class CameraFollower : public rclcpp_lifecycle::LifecycleNode +{ +public: + RASPIMOUSE_ROS2_EXAMPLES_PUBLIC + explicit CameraFollower(const rclcpp::NodeOptions & options); + +protected: + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg_image); + void callback_switches(const raspimouse_msgs::msg::Switches::SharedPtr msg); + void on_cmd_vel_timer(); + +private: + cv::VideoCapture cap_; + bool object_is_detected_; + bool enable_following_; + cv::Point2d object_normalized_point_; + double object_normalized_area_; + std::shared_ptr> result_image_pub_; + std::shared_ptr> cmd_vel_pub_; + std::shared_ptr> motor_power_client_; + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp::Subscription::SharedPtr switches_sub_; + rclcpp::TimerBase::SharedPtr cmd_vel_timer_; + + void set_motor_power(const bool motor_on); + std::string mat_type2encoding(const int mat_type) const; + void convert_frame_to_message( + const cv::Mat & frame, + sensor_msgs::msg::Image & msg) const; + + bool detect_line(const cv::Mat & input_frame, cv::Mat & result_frame); + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_activate(const rclcpp_lifecycle::State &); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State &); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_shutdown(const rclcpp_lifecycle::State &); +}; + +} // namespace camera_line_follower + +#endif // RASPIMOUSE_ROS2_EXAMPLES__CAMERA_LINE_FOLLOWER_COMPONENT_HPP_ diff --git a/launch/camera_line_follower.launch.py b/launch/camera_line_follower.launch.py new file mode 100644 index 0000000..2424bc1 --- /dev/null +++ b/launch/camera_line_follower.launch.py @@ -0,0 +1,94 @@ +# 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. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + declare_mouse = DeclareLaunchArgument( + 'mouse', + default_value="true", + description='Launch raspimouse node' + ) + declare_use_camera_node = DeclareLaunchArgument( + 'use_camera_node', + default_value='true', + description='Use camera node.' + ) + declare_video_device = DeclareLaunchArgument( + 'video_device', + default_value='/dev/video0', + description='Set video device.' + ) + + """Generate launch description with multiple components.""" + container = ComposableNodeContainer( + name='camera_line_follower_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + ComposableNode( + package='raspimouse_ros2_examples', + plugin='camera_line_follower::CameraFollower', + name='camera_follower', + extra_arguments=[{'use_intra_process_comms': True}]), + ComposableNode( + package='raspimouse', + plugin='raspimouse::Raspimouse', + name='raspimouse', + parameters=[{'use_light_sensors': False}], + extra_arguments=[{'use_intra_process_comms': True}], + condition=IfCondition(LaunchConfiguration('mouse'))), + ComposableNode( + package='usb_cam', + plugin='usb_cam::UsbCamNode', + name='usb_cam', + remappings=[('image_raw', 'camera/color/image_raw')], + parameters=[ + {'video_device': LaunchConfiguration('video_device')}, + {'frame_id': 'camera_color_optical_frame'}, + {'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'))), + ], + output='screen', + ) + + manager = Node( + name='manager', + package='raspimouse_ros2_examples', + executable='lifecycle_node_manager', + output='screen', + parameters=[{'components': ['raspimouse', 'camera_follower']}] + ) + + return launch.LaunchDescription([ + declare_mouse, + declare_use_camera_node, + declare_video_device, + container, + manager + ]) diff --git a/package.xml b/package.xml index f634699..decb8ad 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ raspimouse_ros2_examples - 2.1.0 + 2.2.0 Raspberry Pi Mouse examples RT Corporation @@ -31,6 +31,7 @@ nav2_map_server slam_toolbox cv_bridge + usb_cam ament_lint_auto ament_lint_common diff --git a/src/camera_line_follower_component.cpp b/src/camera_line_follower_component.cpp new file mode 100644 index 0000000..2acc8b4 --- /dev/null +++ b/src/camera_line_follower_component.cpp @@ -0,0 +1,301 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "raspimouse_ros2_examples/camera_line_follower_component.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#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" + +constexpr auto MIN_BRIGHTNESS_PARAM = "min_brightness"; +constexpr auto MAX_BRIGHTNESS_PARAM = "max_brightness"; +constexpr auto LINEAR_VEL_PARAM = "max_linear_vel"; +constexpr auto ANGULAR_VEL_PARAM = "max_angular_vel"; +constexpr auto AREA_THRESHOLD_PARAM = "area_threshold"; + + +namespace camera_line_follower +{ +using namespace std::chrono_literals; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +CameraFollower::CameraFollower(const rclcpp::NodeOptions & options) +: rclcpp_lifecycle::LifecycleNode("camera_follower", options), + object_is_detected_(false) +{ +} + +void CameraFollower::image_callback(const sensor_msgs::msg::Image::SharedPtr msg_image) +{ + const auto cv_img = cv_bridge::toCvShare(msg_image, msg_image->encoding); + auto result_msg = std::make_unique(); + + cv::Mat frame, result_frame; + cv::cvtColor(cv_img->image, frame, CV_RGB2BGR); + + if (!frame.empty()) { + object_is_detected_ = detect_line(frame, result_frame); + convert_frame_to_message(result_frame, *result_msg); + result_image_pub_->publish(std::move(result_msg)); + } +} + +void CameraFollower::callback_switches(const raspimouse_msgs::msg::Switches::SharedPtr msg) +{ + if (msg->switch0) { + RCLCPP_INFO(this->get_logger(), "Stop following."); + set_motor_power(false); + enable_following_ = false; + } else if (msg->switch2) { + RCLCPP_INFO(this->get_logger(), "Start following."); + set_motor_power(true); + enable_following_ = true; + } +} + +void CameraFollower::on_cmd_vel_timer() +{ + 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_ > get_parameter(AREA_THRESHOLD_PARAM).as_double()) + { + cmd_vel.linear.x = get_parameter(LINEAR_VEL_PARAM).as_double(); + cmd_vel.angular.z = -get_parameter(ANGULAR_VEL_PARAM).as_double() * object_normalized_point_.x; + } else { + 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(cmd_vel); + cmd_vel_pub_->publish(std::move(msg)); +} + +void CameraFollower::set_motor_power(const bool motor_on) +{ + if (motor_power_client_ == nullptr) { + RCLCPP_ERROR( + this->get_logger(), + "Service motor_power is not avaliable."); + return; + } + auto request = std::make_shared(); + request->data = motor_on; + auto future_result = motor_power_client_->async_send_request(request); +} + +// Ref: https://github.com/ros2/demos/blob/dashing/image_tools/src/cam2image.cpp +std::string CameraFollower::mat_type2encoding(const int mat_type) const +{ + switch (mat_type) { + case CV_8UC1: + return "mono8"; + case CV_8UC3: + return "bgr8"; + case CV_16SC1: + return "mono16"; + case CV_8UC4: + return "rgba8"; + default: + throw std::runtime_error("Unsupported encoding type"); + } +} + +// Ref: https://github.com/ros2/demos/blob/dashing/image_tools/src/cam2image.cpp +void CameraFollower::convert_frame_to_message( + const cv::Mat & frame, sensor_msgs::msg::Image & msg) const +{ + // copy cv information into ros message + msg.height = frame.rows; + msg.width = frame.cols; + msg.encoding = mat_type2encoding(frame.type()); + msg.step = static_cast(frame.step); + size_t size = frame.step * frame.rows; + msg.data.resize(size); + memcpy(&msg.data[0], frame.data, size); + msg.header.frame_id = "camera_frame"; +} + +bool CameraFollower::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, + get_parameter(MIN_BRIGHTNESS_PARAM).as_int(), + get_parameter(MAX_BRIGHTNESS_PARAM).as_int(), + extracted_bin); + input_frame.copyTo(result_frame, extracted_bin); + + // Remove noise with morphology transformation + cv::Mat morph_bin; + cv::morphologyEx(extracted_bin, morph_bin, cv::MORPH_CLOSE, cv::Mat()); + + // Extracting contours + std::vector> contours; + std::vector hierarchy; + cv::findContours(morph_bin, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); + + // Extracting the largest contours + double max_area = 0; + int max_area_index = -1; + for (unsigned int index = 0; index < contours.size(); index++) { + double area = cv::contourArea(contours.at(index)); + if (area > max_area) { + max_area = area; + max_area_index = index; + } + } + + // If the contour exists (if the object exists), find the centroid of the contour + if (max_area_index >= 0) { + cv::Moments mt = cv::moments(contours.at(max_area_index)); + cv::Point mt_point = cv::Point(mt.m10 / mt.m00, mt.m01 / mt.m00); + + // Normalize the centroid coordinates to [-1.0, 1.0]. + object_normalized_point_ = cv::Point2d( + 2.0 * mt_point.x / input_frame.cols - 1.0, + 2.0 * mt_point.y / input_frame.rows - 1.0 + ); + // Normalize the the contour area to [0.0, 1.0]. + object_normalized_area_ = max_area / (input_frame.rows * input_frame.cols); + + std::string text = "Area:" + std::to_string(object_normalized_area_ * 100) + "%"; + cv::drawContours( + result_frame, contours, max_area_index, + cv::Scalar(0, 255, 0), 2, cv::LINE_4, hierarchy); + 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, 255, 255), 2); + return true; + } else { + return false; + } +} + +CallbackReturn CameraFollower::on_configure(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(this->get_logger(), "on_configure() is called."); + + cmd_vel_timer_ = create_wall_timer(50ms, std::bind(&CameraFollower::on_cmd_vel_timer, this)); + // Don't actually start publishing data until activated + cmd_vel_timer_->cancel(); + + motor_power_client_ = create_client("motor_power"); + if (!motor_power_client_->wait_for_service(5s)) { + RCLCPP_ERROR( + this->get_logger(), + "Service motor_power is not avaliable."); + return CallbackReturn::FAILURE; + } + + result_image_pub_ = create_publisher("result_image", 1); + cmd_vel_pub_ = create_publisher("cmd_vel", 1); + image_sub_ = create_subscription( + "camera/color/image_raw", rclcpp::SensorDataQoS(), + std::bind(&CameraFollower::image_callback, this, std::placeholders::_1)); + switches_sub_ = create_subscription( + "switches", 1, std::bind(&CameraFollower::callback_switches, this, std::placeholders::_1)); + + // Set parameter defaults + declare_parameter(MIN_BRIGHTNESS_PARAM, 0); + declare_parameter(MAX_BRIGHTNESS_PARAM, 90); + declare_parameter(LINEAR_VEL_PARAM, 0.05); + declare_parameter(ANGULAR_VEL_PARAM, 0.8); + declare_parameter(AREA_THRESHOLD_PARAM, 0.2); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn CameraFollower::on_activate(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(this->get_logger(), "on_activate() is called."); + + result_image_pub_->on_activate(); + cmd_vel_pub_->on_activate(); + cmd_vel_timer_->reset(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn CameraFollower::on_deactivate(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(this->get_logger(), "on_deactivate() is called."); + + set_motor_power(false); + + result_image_pub_->on_deactivate(); + cmd_vel_pub_->on_deactivate(); + cmd_vel_timer_->cancel(); + + object_is_detected_ = false; + + return CallbackReturn::SUCCESS; +} + +CallbackReturn CameraFollower::on_cleanup(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(this->get_logger(), "on_cleanup() is called."); + + set_motor_power(false); + + result_image_pub_.reset(); + cmd_vel_pub_.reset(); + cmd_vel_timer_.reset(); + image_sub_.reset(); + switches_sub_.reset(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn CameraFollower::on_shutdown(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(this->get_logger(), "on_shutdown() is called."); + + set_motor_power(false); + + result_image_pub_.reset(); + cmd_vel_pub_.reset(); + cmd_vel_timer_.reset(); + image_sub_.reset(); + switches_sub_.reset(); + + return CallbackReturn::SUCCESS; +} + +} // namespace camera_line_follower + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(camera_line_follower::CameraFollower)