From 7c323abcb0f99469986b564c45a01e52841f5707 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Wed, 1 Nov 2023 18:28:59 +0900 Subject: [PATCH 01/12] =?UTF-8?q?object=5Ftracking=E3=81=AB=E3=81=8A?= =?UTF-8?q?=E3=81=84=E3=81=A6=E7=94=BB=E5=83=8F=E3=83=88=E3=83=94=E3=83=83?= =?UTF-8?q?=E3=82=AF=E3=82=92=E3=82=B5=E3=83=96=E3=82=B9=E3=82=AF=E3=83=A9?= =?UTF-8?q?=E3=82=A4=E3=83=96=E3=81=99=E3=82=8B=E3=82=88=E3=81=86=E3=81=AB?= =?UTF-8?q?=E5=A4=89=E6=9B=B4=20(#43)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * 画像トピックのサブスクライバを実装 * 画像トピックがRGBなのでBGRに変換 * mouseとcamera_nodeのオンオフを実装 * 実機で動作するように変更 * 物体追従のパラメータ調整 * 不要なメンバ変数を削除 * コールバック関数の名前を変更 * コメントアウトしていた不要な行を削除 * rclcpp::SensorDataQoS()を使用するように変更 * std::placeholders::_1を直接書くように修正 * raspimouseノードにComposableNodeを使用 * usb_camノードにもComposableNodeを使用 * 各ノードのuse_intra_process_commsをtrueに変更 * image_pub_を削除 * READMEのobject_trackingのコマンドを修正 * Gazeboの場合のコマンド例を削除 --- CMakeLists.txt | 4 +- README.en.md | 4 +- README.md | 4 +- .../object_tracking_component.hpp | 8 +--- launch/object_tracking.launch.py | 47 +++++++++++++++++-- package.xml | 1 + src/object_tracking_component.cpp | 41 +++++----------- 7 files changed, 65 insertions(+), 44 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9515133..c6238bb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,6 +30,7 @@ find_package(OpenCV REQUIRED) find_package(raspimouse_msgs REQUIRED) find_package(raspimouse REQUIRED) find_package(rt_usb_9axisimu_driver REQUIRED) +find_package(cv_bridge REQUIRED) include_directories(include) @@ -46,7 +47,8 @@ ament_target_dependencies(object_tracking_component std_srvs sensor_msgs geometry_msgs - OpenCV) + OpenCV + cv_bridge) rclcpp_components_register_nodes(object_tracking_component "object_tracking::Tracker") add_library(line_follower_component SHARED diff --git a/README.en.md b/README.en.md index d293e46..aa37f41 100644 --- a/README.en.md +++ b/README.en.md @@ -155,10 +155,10 @@ $ ./configure_camera.bash Then, launch nodes with the following command: ```sh -$ ros2 launch raspimouse_ros2_examples object_tracking.launch.py +$ ros2 launch raspimouse_ros2_examples object_tracking.launch.py video_device:=/dev/video0 ``` -This sample publishes two topics: `raw_image` for the camera image and `result_image` for the object detection image. +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/). diff --git a/README.md b/README.md index 7783b80..c8ef434 100644 --- a/README.md +++ b/README.md @@ -157,10 +157,10 @@ $ ./configure_camera.bash 次のコマンドでノードを起動します。 ```sh -$ ros2 launch raspimouse_ros2_examples object_tracking.launch.py +$ ros2 launch raspimouse_ros2_examples object_tracking.launch.py video_device:=/dev/video0 ``` -カメラ画像は`raw_image`、物体検出画像は`result_image`というトピックとして発行されます。 +カメラ画像は`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/) で表示できます。 diff --git a/include/raspimouse_ros2_examples/object_tracking_component.hpp b/include/raspimouse_ros2_examples/object_tracking_component.hpp index 3524059..7c8dc9c 100644 --- a/include/raspimouse_ros2_examples/object_tracking_component.hpp +++ b/include/raspimouse_ros2_examples/object_tracking_component.hpp @@ -38,23 +38,19 @@ class Tracker : public rclcpp_lifecycle::LifecycleNode explicit Tracker(const rclcpp::NodeOptions & options); protected: - void on_image_timer(); + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg_image); void on_cmd_vel_timer(); private: cv::VideoCapture cap_; - int device_index_; - double image_width_; - double image_height_; bool object_is_detected_; cv::Point2d object_normalized_point_; double object_normalized_area_; geometry_msgs::msg::Twist cmd_vel_; - std::shared_ptr> image_pub_; std::shared_ptr> result_image_pub_; std::shared_ptr> cmd_vel_pub_; std::shared_ptr> motor_power_client_; - rclcpp::TimerBase::SharedPtr image_timer_; + rclcpp::Subscription::SharedPtr image_sub_; rclcpp::TimerBase::SharedPtr cmd_vel_timer_; std::string mat_type2encoding(int mat_type); diff --git a/launch/object_tracking.launch.py b/launch/object_tracking.launch.py index 183c1e6..e29a51a 100644 --- a/launch/object_tracking.launch.py +++ b/launch/object_tracking.launch.py @@ -13,12 +13,31 @@ # 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='object_tracking_container', @@ -29,12 +48,27 @@ def generate_launch_description(): ComposableNode( package='raspimouse_ros2_examples', plugin='object_tracking::Tracker', - name='tracker'), + name='tracker', + extra_arguments=[{'use_intra_process_comms': True}]), ComposableNode( package='raspimouse', plugin='raspimouse::Raspimouse', name='raspimouse', - parameters=[{'use_light_sensors': False}]), + 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'} + ], + extra_arguments=[{'use_intra_process_comms': True}], + condition=IfCondition(LaunchConfiguration('use_camera_node'))), ], output='screen', ) @@ -45,7 +79,12 @@ def generate_launch_description(): executable='lifecycle_node_manager', output='screen', parameters=[{'components': ['raspimouse', 'tracker']}] - ) - return launch.LaunchDescription([container, manager]) + return launch.LaunchDescription([ + declare_mouse, + declare_use_camera_node, + declare_video_device, + container, + manager + ]) diff --git a/package.xml b/package.xml index fe749fb..5145b61 100644 --- a/package.xml +++ b/package.xml @@ -30,6 +30,7 @@ hls_lfcd_lds_driver nav2_map_server slam_toolbox + cv_bridge ament_lint_auto ament_lint_common diff --git a/src/object_tracking_component.cpp b/src/object_tracking_component.cpp index cc10600..0673839 100644 --- a/src/object_tracking_component.cpp +++ b/src/object_tracking_component.cpp @@ -25,6 +25,7 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.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; @@ -34,28 +35,25 @@ namespace object_tracking Tracker::Tracker(const rclcpp::NodeOptions & options) : rclcpp_lifecycle::LifecycleNode("tracker", options), - device_index_(0), image_width_(320), image_height_(240), object_is_detected_(false) { } -void Tracker::on_image_timer() +void Tracker::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(); auto result_msg = std::make_unique(); msg->is_bigendian = false; result_msg->is_bigendian = false; cv::Mat frame, result_frame; - cap_ >> frame; + cv::cvtColor(cv_img->image, frame, CV_RGB2BGR); if (!frame.empty()) { tracking(frame, result_frame); convert_frame_to_message(result_frame, *result_msg); result_image_pub_->publish(std::move(result_msg)); - - convert_frame_to_message(frame, *msg); - image_pub_->publish(std::move(msg)); } } @@ -63,8 +61,8 @@ void Tracker::on_cmd_vel_timer() { const double LINEAR_VEL = -0.5; // unit: m/s const double ANGULAR_VEL = -0.8; // unit: rad/s - const double TARGET_AREA = 0.3; // 0.0 ~ 1.0 - const double OBJECT_AREA_THRESHOLD = 0.06; // 0.0 ~ 1.0 + 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 // when the number of pixels of the object is greater than the threshold. @@ -117,7 +115,7 @@ void Tracker::tracking(const cv::Mat & input_frame, cv::Mat & result_frame) cv::Mat hsv; cv::cvtColor(input_frame, hsv, cv::COLOR_BGR2HSV); cv::Mat extracted_bin; - cv::inRange(hsv, cv::Scalar(9, 100, 100), cv::Scalar(29, 255, 255), extracted_bin); // Orange + cv::inRange(hsv, cv::Scalar(0, 100, 100), cv::Scalar(29, 255, 255), extracted_bin); // Red-Orange // cv::inRange(hsv, cv::Scalar(60, 100, 100), cv::Scalar(80, 255, 255), extracted_bin); // Green // cv::inRange(hsv, cv::Scalar(100, 100, 100), cv::Scalar(120, 255, 255), extracted_bin); // Blue input_frame.copyTo(result_frame, extracted_bin); @@ -173,24 +171,15 @@ CallbackReturn Tracker::on_configure(const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "on_configure() is called."); - image_timer_ = create_wall_timer(100ms, std::bind(&Tracker::on_image_timer, this)); cmd_vel_timer_ = create_wall_timer(50ms, std::bind(&Tracker::on_cmd_vel_timer, this)); // Don't actually start publishing data until activated - image_timer_->cancel(); cmd_vel_timer_->cancel(); - image_pub_ = create_publisher("raw_image", 1); result_image_pub_ = create_publisher("result_image", 1); cmd_vel_pub_ = create_publisher("cmd_vel", 1); - - // Initialize OpenCV video capture stream. - cap_.open(device_index_); - cap_.set(cv::CAP_PROP_FRAME_WIDTH, image_width_); - cap_.set(cv::CAP_PROP_FRAME_HEIGHT, image_height_); - if (!cap_.isOpened()) { - RCLCPP_ERROR(this->get_logger(), "Could not open video stream"); - return CallbackReturn::FAILURE; - } + image_sub_ = create_subscription( + "camera/color/image_raw", rclcpp::SensorDataQoS(), + std::bind(&Tracker::image_callback, this, std::placeholders::_1)); return CallbackReturn::SUCCESS; } @@ -210,10 +199,8 @@ CallbackReturn Tracker::on_activate(const rclcpp_lifecycle::State &) request->data = true; auto future_result = motor_power_client_->async_send_request(request); - image_pub_->on_activate(); result_image_pub_->on_activate(); cmd_vel_pub_->on_activate(); - image_timer_->reset(); cmd_vel_timer_->reset(); return CallbackReturn::SUCCESS; @@ -222,10 +209,8 @@ CallbackReturn Tracker::on_activate(const rclcpp_lifecycle::State &) CallbackReturn Tracker::on_deactivate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "on_deactivate() is called."); - image_pub_->on_deactivate(); result_image_pub_->on_deactivate(); cmd_vel_pub_->on_deactivate(); - image_timer_->cancel(); cmd_vel_timer_->cancel(); object_is_detected_ = false; @@ -238,11 +223,10 @@ CallbackReturn Tracker::on_cleanup(const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "on_cleanup() is called."); - image_pub_.reset(); result_image_pub_.reset(); cmd_vel_pub_.reset(); - image_timer_.reset(); cmd_vel_timer_.reset(); + image_sub_.reset(); return CallbackReturn::SUCCESS; } @@ -251,11 +235,10 @@ CallbackReturn Tracker::on_shutdown(const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "on_shutdown() is called."); - image_pub_.reset(); result_image_pub_.reset(); cmd_vel_pub_.reset(); - image_timer_.reset(); cmd_vel_timer_.reset(); + image_sub_.reset(); return CallbackReturn::SUCCESS; } From 6e839e7f57b38e66146439db2d4028cbf69a5519 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Mon, 6 Nov 2023 11:36:48 +0900 Subject: [PATCH 02/12] =?UTF-8?q?README=E3=81=ABGazebo=E3=81=A7=E3=82=82?= =?UTF-8?q?=E5=AE=9F=E8=A1=8C=E3=81=A7=E3=81=8D=E3=82=8B=E3=81=93=E3=81=A8?= =?UTF-8?q?=E3=82=92=E8=BF=BD=E8=A8=98=20(#44)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * READMEにGazeboでも動作することを追記 * 英語版のREADMEにもGazeboの情報を追記 * READMEのサンプル集のリンクを修正 * READMEのraspimouse_simのリンクを修正 --- README.en.md | 4 +++- README.md | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/README.en.md b/README.en.md index aa37f41..e586116 100644 --- a/README.en.md +++ b/README.en.md @@ -6,7 +6,9 @@ ROS 2 examples for Raspberry Pi Mouse. -ROS1 examples is [here](https://github.com/rt-net/raspimouse_ros_examples). +ROS1 examples is [here](https://github.com/rt-net/raspimouse_ros_examples/blob/master/README.en.md). + +To run on Gazebo, click [here](https://github.com/rt-net/raspimouse_sim/blob/ros2/README.en.md). diff --git a/README.md b/README.md index c8ef434..2b82003 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,9 @@ Raspberry Pi MouseのROS 2サンプルコード集です。 -ROS1のサンプルコード集は[こちら](https://github.com/rt-net/raspimouse_ros_examples)。 +ROS1のサンプルコード集は[こちら](https://github.com/rt-net/raspimouse_ros_examples/blob/master/README.md)。 + +Gazebo（シミュレータ）でも動作します。詳細は[こちら](https://github.com/rt-net/raspimouse_sim/blob/ros2/README.md)。 From f35d35a2f7723382e8fd2313ac69187432a4d5e3 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Tue, 7 Nov 2023 14:58:23 +0900 Subject: [PATCH 03/12] =?UTF-8?q?=E3=83=AA=E3=83=AA=E3=83=BC=E3=82=B9?= =?UTF-8?q?=E3=81=AE=E3=81=9F=E3=82=81=E3=81=ABCHANGELOG.rst=E3=81=A8packa?= =?UTF-8?q?ge.xml=E3=82=92=E6=9B=B4=E6=96=B0=20(#45)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * リリースのためにCHANGELOG.rstを更新 * 2.1.0 --- CHANGELOG.rst | 6 ++++++ package.xml | 2 +- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 59f9cfc..ea7813e 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package raspimouse_ros2_examples ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.1.0 (2023-11-07) +------------------ +* READMEにGazeboでも実行できることを追記 (`#44 `_) +* object_trackingにおいて画像トピックをサブスクライブするように変更 (`#43 `_) +* Contributors: YusukeKato + 2.0.0 (2023-08-03) ------------------ * Humble対応 (`#41 `_) diff --git a/package.xml b/package.xml index 5145b61..f634699 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ raspimouse_ros2_examples - 2.0.0 + 2.1.0 Raspberry Pi Mouse examples RT Corporation From 153c25c7d69616bfa9f209c5583990b432ff4db3 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Wed, 6 Dec 2023 18:36:20 +0900 Subject: [PATCH 04/12] =?UTF-8?q?RGB=E3=82=AB=E3=83=A1=E3=83=A9=E3=81=AB?= =?UTF-8?q?=E3=82=88=E3=82=8B=E3=83=A9=E3=82=A4=E3=83=B3=E3=83=88=E3=83=AC?= =?UTF-8?q?=E3=83=BC=E3=82=B9=E3=81=AE=E5=AE=9F=E8=A3=85=20(#47)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * カメラによるライントレース用のファイルを作成 * 実行可能な状態に更新 * ライントレースが実行可能な状態に更新 * スイッチ0を押してcmd_velの配信をオンオフできるように変更 * スイッチの処理を修正 * colcon testが通るように修正 * Copyrightの文面を修正 Co-authored-by: Shota Aoki * Copyrightの文面を修正 Co-authored-by: Shota Aoki * Copyrightの文面を修正 Co-authored-by: Shota Aoki * 使用されていないmsgを削除 Co-authored-by: Shota Aoki * usingをnamespace内で使用するように変更 * 変数にconstを付ける * is_bigendianの行を削除 * 内部変数を変更しない関数にconstを付ける Co-authored-by: Shota Aoki * ソースコードのほうでもconstを付ける * cmd_velを関数内で宣言する * constをconstexprに変更 Co-authored-by: Shota Aoki * コメントを修正 * スイッチの処理を修正 * following()をdetecting_lineに変更 * 検出結果の文字の色を青から白に変更 * コメントを修正 Co-authored-by: Shota Aoki * 返り値をbool型に変更 * 関数にconstを付ける --------- Co-authored-by: Shota Aoki --- CMakeLists.txt | 19 ++ .../camera_line_follower_component.hpp | 80 ++++++ launch/camera_line_follower.launch.py | 90 ++++++ src/camera_line_follower_component.cpp | 267 ++++++++++++++++++ 4 files changed, 456 insertions(+) create mode 100644 include/raspimouse_ros2_examples/camera_line_follower_component.hpp create mode 100644 launch/camera_line_follower.launch.py create mode 100644 src/camera_line_follower_component.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index c6238bb..10db426 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::Camera_Follower") + 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/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..0e7a756 --- /dev/null +++ b/include/raspimouse_ros2_examples/camera_line_follower_component.hpp @@ -0,0 +1,80 @@ +// 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 Camera_Follower : public rclcpp_lifecycle::LifecycleNode +{ +public: + RASPIMOUSE_ROS2_EXAMPLES_PUBLIC + explicit Camera_Follower(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_; + + 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 detecting_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..f43c09f --- /dev/null +++ b/launch/camera_line_follower.launch.py @@ -0,0 +1,90 @@ +# 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::Camera_Follower', + 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'} + ], + 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/src/camera_line_follower_component.cpp b/src/camera_line_follower_component.cpp new file mode 100644 index 0000000..cc7ba99 --- /dev/null +++ b/src/camera_line_follower_component.cpp @@ -0,0 +1,267 @@ +// 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" + + +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), + object_is_detected_(false) +{ +} + +void Camera_Follower::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_ = 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) +{ + 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() +{ + 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; + + // 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; + } 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)); +} + +// Ref: https://github.com/ros2/demos/blob/dashing/image_tools/src/cam2image.cpp +std::string Camera_Follower::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 Camera_Follower::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 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; + cv::cvtColor(input_frame, gray, cv::COLOR_BGR2GRAY); + cv::Mat extracted_bin; + cv::inRange(gray, 0, 50, 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_NONE); + + // 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 Camera_Follower::on_configure(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(this->get_logger(), "on_configure() is called."); + + cmd_vel_timer_ = create_wall_timer(50ms, std::bind(&Camera_Follower::on_cmd_vel_timer, this)); + // Don't actually start publishing data until activated + cmd_vel_timer_->cancel(); + + 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(&Camera_Follower::image_callback, this, std::placeholders::_1)); + switches_sub_ = create_subscription( + "switches", 1, std::bind(&Camera_Follower::callback_switches, this, std::placeholders::_1)); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn Camera_Follower::on_activate(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(this->get_logger(), "on_activate() is called."); + + 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; + } + auto request = std::make_shared(); + request->data = true; + auto future_result = motor_power_client_->async_send_request(request); + + result_image_pub_->on_activate(); + cmd_vel_pub_->on_activate(); + cmd_vel_timer_->reset(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn Camera_Follower::on_deactivate(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(this->get_logger(), "on_deactivate() is called."); + result_image_pub_->on_deactivate(); + cmd_vel_pub_->on_deactivate(); + cmd_vel_timer_->cancel(); + + object_is_detected_ = false; + + return CallbackReturn::SUCCESS; +} + +CallbackReturn Camera_Follower::on_cleanup(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(this->get_logger(), "on_cleanup() is called."); + + result_image_pub_.reset(); + cmd_vel_pub_.reset(); + cmd_vel_timer_.reset(); + image_sub_.reset(); + switches_sub_.reset(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn Camera_Follower::on_shutdown(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(this->get_logger(), "on_shutdown() is called."); + + 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::Camera_Follower) From 6ce932b845a0f1d724041613cb4f70e3a3b1f470 Mon Sep 17 00:00:00 2001 From: Shota Aoki Date: Mon, 15 Jan 2024 09:51:42 +0900 Subject: [PATCH 05/12] Add usb_cam dependency (#48) --- package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/package.xml b/package.xml index f634699..67d8199 100644 --- a/package.xml +++ b/package.xml @@ -31,6 +31,7 @@ nav2_map_server slam_toolbox cv_bridge + usb_cam ament_lint_auto ament_lint_common From 8eedd499d433539c1a1c4f54779685be98f8454d Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Mon, 15 Jan 2024 11:36:50 +0900 Subject: [PATCH 06/12] Change threthold of line detection --- src/camera_line_follower_component.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/camera_line_follower_component.cpp b/src/camera_line_follower_component.cpp index cc7ba99..08abcc9 100644 --- a/src/camera_line_follower_component.cpp +++ b/src/camera_line_follower_component.cpp @@ -131,7 +131,7 @@ 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, 50, extracted_bin); + cv::inRange(gray, 0, 100, extracted_bin); input_frame.copyTo(result_frame, extracted_bin); // Remove noise with morphology transformation From 85b3bf01c2e22f86318ddae9dd96369630f396f2 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Mon, 5 Feb 2024 18:52:19 +0900 Subject: [PATCH 07/12] =?UTF-8?q?=E3=82=AB=E3=83=A1=E3=83=A9=E3=83=A9?= =?UTF-8?q?=E3=82=A4=E3=83=B3=E3=83=88=E3=83=AC=E3=83=BC=E3=82=B9=E3=82=92?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3=20(#49)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * READMEにカメラライントレースの情報を追記 * リサイズした画像に変更 * 目次にカメラライントレースの項目を追加 * rqt_image_viewのリンクが切れていたので更新 * usb_camの配信頻度を30FPSから10FPSに変更 * READMEにカメラライントレースのスイッチ操作の説明を追加 * usb_camの画像サイズを320x240に設定 * adaptiveThreshold関数で画像を2値化 * 速度調整&inRange関数使用&findContours関数の引数を変更 * 2値化のしきい値をパラメータで設定 * デバッグ用の出力を削除 * コードの体裁を整える * READMEに2値化のしきい値のパラメータの設定方法を追加 * デバッグ用に使用していた係数を削除 * detecting_line()をdetect_line()に変更 * READMEのConfigureの項目を削除 * シミュレータ環境でライントレースが行えるように2値化のしきい値を調整 --- README.en.md | 59 ++++++++++++++++++- README.md | 59 ++++++++++++++++++- .../camera_line_follower_component.hpp | 2 +- launch/camera_line_follower.launch.py | 6 +- src/camera_line_follower_component.cpp | 24 +++++--- 5 files changed, 139 insertions(+), 11 deletions(-) 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; } From 29ad3514de93ac98b02f1f5f606684bb742e90d0 Mon Sep 17 00:00:00 2001 From: Shota Aoki Date: Tue, 6 Feb 2024 18:39:15 +0900 Subject: [PATCH 08/12] Add velocity parameters for camera_line_follower (#50) * Add velocity parameters for camera_line_follower * update parameter names * Update README.en.md Co-authored-by: YusukeKato --------- Co-authored-by: YusukeKato --- README.en.md | 12 ++++++++++-- README.md | 12 ++++++++++-- src/camera_line_follower_component.cpp | 22 ++++++++++++---------- 3 files changed, 32 insertions(+), 14 deletions(-) diff --git a/README.en.md b/README.en.md index 95676a6..d6b2622 100644 --- a/README.en.md +++ b/README.en.md @@ -289,14 +289,22 @@ or [rqt_image_view](https://index.ros.org/p/rqt_image_view/). #### Parameters -- `brightness_max_value` +- `max_brightness` - Type: `int` - Default: 90 - Maximum threshold value for image binarisation. -- `brightness_min_value` +- `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. ```sh ros2 param set /camera_follower brightness_max_value 80 diff --git a/README.md b/README.md index 93052cb..d0babaf 100644 --- a/README.md +++ b/README.md @@ -293,14 +293,22 @@ $ ros2 launch raspimouse_ros2_examples camera_line_follower.launch.py video_devi #### Parameters -- `brightness_max_value` +- `max_brightness` - Type: `int` - Default: 90 - 画像の2値化のしきい値の最大値 -- `brightness_min_value` +- `min_brightness` - Type: `int` - Default: 0 - 画像の2値化のしきい値の最小値 +- `max_linear_vel` + - Type: `double` + - Default: 0.05 + - 直進速度の最大値 +- `max_angular_vel` + - Type: `double` + - Default: 0.8 + - 旋回速度の最大値 ```sh ros2 param set /camera_follower brightness_max_value 80 diff --git a/src/camera_line_follower_component.cpp b/src/camera_line_follower_component.cpp index 2c316bb..3067a93 100644 --- a/src/camera_line_follower_component.cpp +++ b/src/camera_line_follower_component.cpp @@ -28,8 +28,10 @@ #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"; +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"; namespace camera_line_follower @@ -71,16 +73,14 @@ void Camera_Follower::callback_switches(const raspimouse_msgs::msg::Switches::Sh void Camera_Follower::on_cmd_vel_timer() { - constexpr double LINEAR_VEL = 0.05; // unit: m/s - constexpr double ANGULAR_VEL = -0.8; // unit: rad/s 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; - cmd_vel.angular.z = ANGULAR_VEL * object_normalized_point_.x; + 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; @@ -135,8 +135,8 @@ bool Camera_Follower::detect_line(const cv::Mat & input_frame, cv::Mat & result_ cv::Mat extracted_bin; cv::inRange( gray, - get_parameter(BRIGHTNESS_MIN_VAL_PARAM).get_value(), - get_parameter(BRIGHTNESS_MAX_VAL_PARAM).get_value(), + get_parameter(MIN_BRIGHTNESS_PARAM).as_int(), + get_parameter(MAX_BRIGHTNESS_PARAM).as_int(), extracted_bin); input_frame.copyTo(result_frame, extracted_bin); @@ -204,8 +204,10 @@ CallbackReturn Camera_Follower::on_configure(const rclcpp_lifecycle::State &) "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); + 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); return CallbackReturn::SUCCESS; } From 930dc29a71171bd18b7d18cad32278102c48e708 Mon Sep 17 00:00:00 2001 From: Shota Aoki Date: Wed, 7 Feb 2024 16:15:13 +0900 Subject: [PATCH 09/12] Update camera line follower: Set motor power with switch input. Add area_threthold param. (#51) * Set motor power via switch input * Add area_threthold param * Update README * Update to pass lint check --- README.en.md | 6 ++- README.md | 6 ++- .../camera_line_follower_component.hpp | 1 + src/camera_line_follower_component.cpp | 48 ++++++++++++++----- 4 files changed, 46 insertions(+), 15 deletions(-) diff --git a/README.en.md b/README.en.md index d6b2622..f856590 100644 --- a/README.en.md +++ b/README.en.md @@ -305,9 +305,13 @@ or [rqt_image_view](https://index.ros.org/p/rqt_image_view/). - 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. ```sh -ros2 param set /camera_follower brightness_max_value 80 +ros2 param set /camera_follower max_brightness 80 ``` [back to example list](#how-to-use-examples) diff --git a/README.md b/README.md index d0babaf..3c9b7c5 100644 --- a/README.md +++ b/README.md @@ -309,9 +309,13 @@ $ ros2 launch raspimouse_ros2_examples camera_line_follower.launch.py video_devi - Type: `double` - Default: 0.8 - 旋回速度の最大値 +- `area_threthold` + - Type: `double` + - Default: 0.20 + - 走行を開始するためのライン面積のしきい値 ```sh -ros2 param set /camera_follower brightness_max_value 80 +ros2 param set /camera_follower max_brightness 80 ``` [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 index 262cacd..044bde1 100644 --- a/include/raspimouse_ros2_examples/camera_line_follower_component.hpp +++ b/include/raspimouse_ros2_examples/camera_line_follower_component.hpp @@ -56,6 +56,7 @@ class Camera_Follower : public rclcpp_lifecycle::LifecycleNode 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, diff --git a/src/camera_line_follower_component.cpp b/src/camera_line_follower_component.cpp index 3067a93..9233f31 100644 --- a/src/camera_line_follower_component.cpp +++ b/src/camera_line_follower_component.cpp @@ -32,6 +32,7 @@ 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 @@ -64,21 +65,24 @@ void Camera_Follower::callback_switches(const raspimouse_msgs::msg::Switches::Sh { 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 Camera_Follower::on_cmd_vel_timer() { - 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) { + 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 { @@ -95,6 +99,19 @@ void Camera_Follower::on_cmd_vel_timer() cmd_vel_pub_->publish(std::move(msg)); } +void Camera_Follower::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 Camera_Follower::mat_type2encoding(const int mat_type) const { @@ -195,6 +212,14 @@ CallbackReturn Camera_Follower::on_configure(const rclcpp_lifecycle::State &) // 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( @@ -208,6 +233,7 @@ CallbackReturn Camera_Follower::on_configure(const rclcpp_lifecycle::State &) 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; } @@ -216,17 +242,6 @@ CallbackReturn Camera_Follower::on_activate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "on_activate() is called."); - 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; - } - auto request = std::make_shared(); - request->data = true; - auto future_result = motor_power_client_->async_send_request(request); - result_image_pub_->on_activate(); cmd_vel_pub_->on_activate(); cmd_vel_timer_->reset(); @@ -237,6 +252,9 @@ CallbackReturn Camera_Follower::on_activate(const rclcpp_lifecycle::State &) CallbackReturn Camera_Follower::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(); @@ -250,6 +268,8 @@ CallbackReturn Camera_Follower::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(); @@ -263,6 +283,8 @@ CallbackReturn Camera_Follower::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(); From afc8cb1a34cd967e71bccbe5e7abe3e4a1449893 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Wed, 7 Feb 2024 17:22:39 +0900 Subject: [PATCH 10/12] =?UTF-8?q?Camera=5FFollower=E3=82=AF=E3=83=A9?= =?UTF-8?q?=E3=82=B9=E3=82=92CameraFollower=E3=81=AB=E5=A4=89=E6=9B=B4=20(?= =?UTF-8?q?#52)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 2 +- .../camera_line_follower_component.hpp | 4 +-- launch/camera_line_follower.launch.py | 2 +- src/camera_line_follower_component.cpp | 34 +++++++++---------- 4 files changed, 21 insertions(+), 21 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 10db426..15c821f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,7 +66,7 @@ ament_target_dependencies(camera_line_follower_component OpenCV cv_bridge raspimouse_msgs) -rclcpp_components_register_nodes(camera_line_follower_component "camera_line_follower::Camera_Follower") +rclcpp_components_register_nodes(camera_line_follower_component "camera_line_follower::CameraFollower") add_library(line_follower_component SHARED src/line_follower_component.cpp) diff --git a/include/raspimouse_ros2_examples/camera_line_follower_component.hpp b/include/raspimouse_ros2_examples/camera_line_follower_component.hpp index 044bde1..35e89ba 100644 --- a/include/raspimouse_ros2_examples/camera_line_follower_component.hpp +++ b/include/raspimouse_ros2_examples/camera_line_follower_component.hpp @@ -32,11 +32,11 @@ namespace camera_line_follower { -class Camera_Follower : public rclcpp_lifecycle::LifecycleNode +class CameraFollower : public rclcpp_lifecycle::LifecycleNode { public: RASPIMOUSE_ROS2_EXAMPLES_PUBLIC - explicit Camera_Follower(const rclcpp::NodeOptions & options); + explicit CameraFollower(const rclcpp::NodeOptions & options); protected: void image_callback(const sensor_msgs::msg::Image::SharedPtr msg_image); diff --git a/launch/camera_line_follower.launch.py b/launch/camera_line_follower.launch.py index fd933d5..2424bc1 100644 --- a/launch/camera_line_follower.launch.py +++ b/launch/camera_line_follower.launch.py @@ -47,7 +47,7 @@ def generate_launch_description(): composable_node_descriptions=[ ComposableNode( package='raspimouse_ros2_examples', - plugin='camera_line_follower::Camera_Follower', + plugin='camera_line_follower::CameraFollower', name='camera_follower', extra_arguments=[{'use_intra_process_comms': True}]), ComposableNode( diff --git a/src/camera_line_follower_component.cpp b/src/camera_line_follower_component.cpp index 9233f31..2acc8b4 100644 --- a/src/camera_line_follower_component.cpp +++ b/src/camera_line_follower_component.cpp @@ -40,13 +40,13 @@ 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) +CameraFollower::CameraFollower(const rclcpp::NodeOptions & options) : rclcpp_lifecycle::LifecycleNode("camera_follower", options), object_is_detected_(false) { } -void Camera_Follower::image_callback(const sensor_msgs::msg::Image::SharedPtr msg_image) +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(); @@ -61,7 +61,7 @@ void Camera_Follower::image_callback(const sensor_msgs::msg::Image::SharedPtr ms } } -void Camera_Follower::callback_switches(const raspimouse_msgs::msg::Switches::SharedPtr msg) +void CameraFollower::callback_switches(const raspimouse_msgs::msg::Switches::SharedPtr msg) { if (msg->switch0) { RCLCPP_INFO(this->get_logger(), "Stop following."); @@ -74,7 +74,7 @@ void Camera_Follower::callback_switches(const raspimouse_msgs::msg::Switches::Sh } } -void Camera_Follower::on_cmd_vel_timer() +void CameraFollower::on_cmd_vel_timer() { geometry_msgs::msg::Twist cmd_vel; @@ -99,7 +99,7 @@ void Camera_Follower::on_cmd_vel_timer() cmd_vel_pub_->publish(std::move(msg)); } -void Camera_Follower::set_motor_power(const bool motor_on) +void CameraFollower::set_motor_power(const bool motor_on) { if (motor_power_client_ == nullptr) { RCLCPP_ERROR( @@ -113,7 +113,7 @@ void Camera_Follower::set_motor_power(const bool motor_on) } // Ref: https://github.com/ros2/demos/blob/dashing/image_tools/src/cam2image.cpp -std::string Camera_Follower::mat_type2encoding(const int mat_type) const +std::string CameraFollower::mat_type2encoding(const int mat_type) const { switch (mat_type) { case CV_8UC1: @@ -130,7 +130,7 @@ std::string Camera_Follower::mat_type2encoding(const int mat_type) const } // Ref: https://github.com/ros2/demos/blob/dashing/image_tools/src/cam2image.cpp -void Camera_Follower::convert_frame_to_message( +void CameraFollower::convert_frame_to_message( const cv::Mat & frame, sensor_msgs::msg::Image & msg) const { // copy cv information into ros message @@ -144,7 +144,7 @@ void Camera_Follower::convert_frame_to_message( msg.header.frame_id = "camera_frame"; } -bool Camera_Follower::detect_line(const cv::Mat & input_frame, cv::Mat & result_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; @@ -204,11 +204,11 @@ bool Camera_Follower::detect_line(const cv::Mat & input_frame, cv::Mat & result_ } } -CallbackReturn Camera_Follower::on_configure(const rclcpp_lifecycle::State &) +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(&Camera_Follower::on_cmd_vel_timer, this)); + 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(); @@ -224,9 +224,9 @@ CallbackReturn Camera_Follower::on_configure(const rclcpp_lifecycle::State &) cmd_vel_pub_ = create_publisher("cmd_vel", 1); image_sub_ = create_subscription( "camera/color/image_raw", rclcpp::SensorDataQoS(), - std::bind(&Camera_Follower::image_callback, this, std::placeholders::_1)); + std::bind(&CameraFollower::image_callback, this, std::placeholders::_1)); switches_sub_ = create_subscription( - "switches", 1, std::bind(&Camera_Follower::callback_switches, this, std::placeholders::_1)); + "switches", 1, std::bind(&CameraFollower::callback_switches, this, std::placeholders::_1)); // Set parameter defaults declare_parameter(MIN_BRIGHTNESS_PARAM, 0); @@ -238,7 +238,7 @@ CallbackReturn Camera_Follower::on_configure(const rclcpp_lifecycle::State &) return CallbackReturn::SUCCESS; } -CallbackReturn Camera_Follower::on_activate(const rclcpp_lifecycle::State &) +CallbackReturn CameraFollower::on_activate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "on_activate() is called."); @@ -249,7 +249,7 @@ CallbackReturn Camera_Follower::on_activate(const rclcpp_lifecycle::State &) return CallbackReturn::SUCCESS; } -CallbackReturn Camera_Follower::on_deactivate(const rclcpp_lifecycle::State &) +CallbackReturn CameraFollower::on_deactivate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "on_deactivate() is called."); @@ -264,7 +264,7 @@ CallbackReturn Camera_Follower::on_deactivate(const rclcpp_lifecycle::State &) return CallbackReturn::SUCCESS; } -CallbackReturn Camera_Follower::on_cleanup(const rclcpp_lifecycle::State &) +CallbackReturn CameraFollower::on_cleanup(const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "on_cleanup() is called."); @@ -279,7 +279,7 @@ CallbackReturn Camera_Follower::on_cleanup(const rclcpp_lifecycle::State &) return CallbackReturn::SUCCESS; } -CallbackReturn Camera_Follower::on_shutdown(const rclcpp_lifecycle::State &) +CallbackReturn CameraFollower::on_shutdown(const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "on_shutdown() is called."); @@ -298,4 +298,4 @@ CallbackReturn Camera_Follower::on_shutdown(const rclcpp_lifecycle::State &) #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(camera_line_follower::Camera_Follower) +RCLCPP_COMPONENTS_REGISTER_NODE(camera_line_follower::CameraFollower) From ec57501911e3999050990c53da3781fa82df37ea Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Thu, 29 Feb 2024 20:14:33 +0900 Subject: [PATCH 11/12] =?UTF-8?q?README=E3=81=ABSLAM&Navigation=E3=83=91?= =?UTF-8?q?=E3=83=83=E3=82=B1=E3=83=BC=E3=82=B8=E3=81=AE=E6=A1=88=E5=86=85?= =?UTF-8?q?=E3=82=92=E8=BF=BD=E5=8A=A0=20(#53)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * READMEにSLAM&Navigationパッケージの案内を追加 * READMEからSLAMの実行方法を削除 --- README.en.md | 76 +------------------------------------------------ README.md | 80 +--------------------------------------------------- 2 files changed, 2 insertions(+), 154 deletions(-) diff --git a/README.en.md b/README.en.md index f856590..4934b21 100644 --- a/README.en.md +++ b/README.en.md @@ -322,81 +322,7 @@ ros2 param set /camera_follower max_brightness 80 -This is an example to use LiDAR and [slam_toolbox](https://github.com/SteveMacenski/slam_toolbox) for SLAM (Simultaneous Localization And Mapping). - -#### 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) - -#### Installation - -Install a LiDAR to the Raspberry Pi Mouse. - - -- LDS-01 - - - -#### How to use - -Launch nodes on Raspberry Pi Mouse with the following command: - -```sh -# LDS -$ ros2 launch raspimouse_ros2_examples mouse_with_lidar.launch.py lidar:=lds -``` - -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 -``` - -Then, launch the slam_toolbox package (on a remote computer recommend) with the following command: - -```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: - -```sh -$ mkdir ~/maps -$ ros2 run nav2_map_server map_saver_cli -f ~/maps/mymap --ros-args -p save_map_timeout:=10000.0 -``` - -#### 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. - -#### Configure Odometry calculation - -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. - -This improves the accuracy of self-localization. - -```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 3c9b7c5..fb8d1f4 100644 --- a/README.md +++ b/README.md @@ -326,85 +326,7 @@ ros2 param set /camera_follower max_brightness 80 -LiDARと[slam_toolbox](https://github.com/SteveMacenski/slam_toolbox) -を使ってSLAM（自己位置推定と地図作成）を行うサンプルです。 - -#### 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) - -#### Installation - -Raspberry Pi MouseにLiDARを取り付けます。 - - -- LDS-01 - - - -#### How to use - -Raspberry Pi Mouse上で次のコマンドでノードを起動します。 - -```sh -# LDS -$ ros2 launch raspimouse_ros2_examples mouse_with_lidar.launch.py lidar:=lds -``` - -Raspberry Pi Mouseを動かすため`teleop_joy.launch.py`を起動します - -```sh -# Use DUALSHOCK 3 -$ ros2 launch raspimouse_ros2_examples teleop_joy.launch.py joydev:="/dev/input/js0" joyconfig:=dualshock3 mouse:=false -``` - -次のコマンドでslam_toolboxパッケージを起動します。（Remote computerでの実行推奨） - -```sh -$ ros2 launch raspimouse_ros2_examples slam.launch.py -``` - -Raspberry Pi Mouseを動かして地図を作成します。 - -次のコマンドで作成した地図を保存します。 - -```sh -$ mkdir ~/maps -$ ros2 run nav2_map_server map_saver_cli -f ~/maps/mymap --ros-args -p save_map_timeout:=10000.0 -``` - -#### 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`）を計算します。 - -これは自己位置推定の精度を向上させます。 - -```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) From d685bc8c5a5fd1a36c94d641667b5eaaf5f5dd4c Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Tue, 5 Mar 2024 19:07:27 +0900 Subject: [PATCH 12/12] =?UTF-8?q?2.2.0=E3=83=AA=E3=83=AA=E3=83=BC=E3=82=B9?= =?UTF-8?q?=E3=81=AE=E3=81=9F=E3=82=81=E3=81=ABCHANGELOG.rst=E3=81=A8packa?= =?UTF-8?q?ge.xml=E3=82=92=E6=9B=B4=E6=96=B0=20(#54)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * リリースのためにCHANGELOGを更新 * 2.2.0 --- CHANGELOG.rst | 13 +++++++++++++ package.xml | 2 +- 2 files changed, 14 insertions(+), 1 deletion(-) 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/package.xml b/package.xml index 67d8199..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