From b0ac05587321a77950a27462e061434448b9247a Mon Sep 17 00:00:00 2001 From: Kazushi Kurasawa Date: Mon, 25 Nov 2024 10:08:08 +0900 Subject: [PATCH] =?UTF-8?q?ROS=202=20Jazzy=E5=AF=BE=E5=BF=9C=20(#62)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * The header file "cv_bridge.h" in the cv_bridge package has been renamed to "cv_bridge.hpp". This change has been applied to all corresponding "#include" statements. * Replaced "Twist" with "TwistStamped" in "joystick_control.py" * Replaced "Twist" with "TwistStamped" for "camera_line_follower.launch.py" * Replaced "Twist" with "TwistStamped" for "object_tracking.launch.py" * Replaced "Twist" with "TwistStamped" (Changed in all instances, revert if any unexpected issues occur, "revert"). * Modify CI to ensure compatibility with the act tool * Add author * Update README for Support Jazzy * Add CI settings for Rolling * Revert "Add CI settings for Rolling" This reverts commit 5996db3226d4d2340659d48411bb86b8562599f7. * Replace ROS_REPO: [ros] with ROS_REPO: [main] * Add rosinstall for CI * Adapt clang-format with Google style * Fix order of include in c file * Revert "Add rosinstall for CI" This reverts commit b9605cd9fe072790801e70af384c22851083c28d. * Reapply "Add rosinstall for CI" This reverts commit a06b8a9e68346bccf4a36196962cf368b5bd13e2. * Revert "Fix order of include in c file" This reverts commit 356a417c83db166834756630dac80638e5d534fa. * Revert "Adapt clang-format with Google style" This reverts commit 0a420431a45f3d96bc840e58502d364106c554f0. * fix typo of distribution name * Applied "ament_clang_format" and passed cpplint, with manual adjustments made to the "include" order * Adapted ruff formetter * fix "import" module order * Adaptted "ament_uncrustify" * Update README.en.md fo Jazzy * Fix pictures and page links * Fix images for README in japanese * Update Copyright years * Fix Indent and replace .h with .hpp * FIxed autho name * Add warnings for camera settings --- .github/workflows/.ci.rosinstall | 8 + .github/workflows/industrial_ci.yml | 13 +- README.en.md | 35 +++-- README.md | 43 +++--- .../camera_line_follower_component.hpp | 33 ++-- .../direction_controller_component.hpp | 11 +- .../line_follower_component.hpp | 29 ++-- .../object_tracking_component.hpp | 35 ++--- ...ility_control.h => visibility_control.hpp} | 8 +- launch/camera_line_follower.launch.py | 107 +++++++------ launch/direction_controller.launch.py | 29 ++-- launch/line_follower.launch.py | 6 +- launch/mouse_with_lidar.launch.py | 41 +++-- launch/object_tracking.launch.py | 99 ++++++------ launch/slam.launch.py | 21 +-- launch/teleop_joy.launch.py | 55 ++++--- package.xml | 2 + raspimouse_ros2_examples/joystick_control.py | 145 +++++++++++------- src/camera_line_follower_component.cpp | 56 +++---- src/direction_controller.cpp | 2 +- src/direction_controller_component.cpp | 29 ++-- src/lifecycle_node_manager.cpp | 80 ++++------ src/line_follower.cpp | 2 +- src/line_follower_component.cpp | 50 +++--- src/object_tracking_component.cpp | 44 +++--- 25 files changed, 510 insertions(+), 473 deletions(-) create mode 100644 .github/workflows/.ci.rosinstall rename include/raspimouse_ros2_examples/{visibility_control.h => visibility_control.hpp} (89%) diff --git a/.github/workflows/.ci.rosinstall b/.github/workflows/.ci.rosinstall new file mode 100644 index 0000000..a454926 --- /dev/null +++ b/.github/workflows/.ci.rosinstall @@ -0,0 +1,8 @@ +- git: + uri: https://github.com/rt-net/raspimouse2.git + local-name: raspimouse2 + version: jazzy +- git: + uri: https://github.com/rt-net/rt_usb_9axisimu_driver.git + local-name: rt_usb_9axisimu_driver + version: jazzy \ No newline at end of file diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 62ebd57..ec471fc 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -12,16 +12,21 @@ on: schedule: - cron: "0 2 * * 0" # Weekly on Sundays at 02:00 +env: + UPSTREAM_WORKSPACE: .github/workflows/.ci.rosinstall + jobs: industrial_ci: strategy: fail-fast: false matrix: - env: - - { ROS_DISTRO: humble, ROS_REPO: ros } + ROS_DISTRO: [jazzy] + ROS_REPO: [main] runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: "ros-industrial/industrial_ci@master" - env: ${{ matrix.env }} + env: + ROS_DISTRO: ${{matrix.ROS_DISTRO}} + ROS_REPO: ${{matrix.ROS_REPO}} diff --git a/README.en.md b/README.en.md index 4934b21..fa2a296 100644 --- a/README.en.md +++ b/README.en.md @@ -14,25 +14,24 @@ To run on Gazebo, click [here](https://github.com/rt-net/raspimouse_sim/blob/ros ## Supported ROS 2 distributions -- [Foxy](https://github.com/rt-net/raspimouse_ros2_examples/tree/foxy-devel) -- Humble (This branch) +- [Humble](https://github.com/rt-net/raspimouse_ros2_examples/tree/humble) +- [Jazzy](https://github.com/rt-net/raspimouse_ros2_examples/tree/jazzy) (This branch) ## Requirements - Raspberry Pi Mouse - https://rt-net.jp/products/raspberrypimousev3/ - Linux OS - - Ubuntu server 22.04 - - https://ubuntu.com/download/raspberry-pi + - Ubuntu server 24.04 - Device Driver - [rt-net/RaspberryPiMouse](https://github.com/rt-net/RaspberryPiMouse) - ROS - - [Humble Hawksbill](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) + - [Jazzy Jalisco](https://docs.ros.org/en/jazzy/index.html) - Raspberry Pi Mouse ROS 2 package - https://github.com/rt-net/raspimouse2 - Remote Computer (Optional) - ROS - - [Humble Hawksbill](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) + - [Jazzy Jalisco](https://docs.ros.org/en/jazzy/index.html) - Raspberry Pi Mouse ROS 2 package - https://github.com/rt-net/raspimouse2 @@ -41,7 +40,7 @@ To run on Gazebo, click [here](https://github.com/rt-net/raspimouse_sim/blob/ros ```sh $ cd ~/ros2_ws/src # Clone package -$ git clone -b $ROS_DISTRO-devel https://github.com/rt-net/raspimouse_ros2_examples.git +$ git clone -b $ROS_DISTRO https://github.com/rt-net/raspimouse_ros2_examples.git # Install dependencies $ rosdep install -r -y --from-paths . --ignore-src @@ -71,7 +70,7 @@ This repository is licensed under the Apache 2.0, see [LICENSE](./LICENSE) for d This is an example to use joystick controller to control a Raspberry Pi Mouse. -#### Requirements +#### Requirements - Joystick Controller - [Logicool Wireless Gamepad F710](https://gaming.logicool.co.jp/ja-jp/products/gamepads/f710-wireless-gamepad.html#940-0001440) @@ -122,7 +121,7 @@ button_cmd_enable : 4 [back to example list](#how-to-use-examples) ---- +--- ### object_tracking @@ -130,7 +129,7 @@ button_cmd_enable : 4 This is an example to use RGB camera images and OpenCV library for object tracking. -#### Requirements +#### Requirements - Web camera - [Logicool HD WEBCAM C310N](https://www.logicool.co.jp/ja-jp/product/hd-webcam-c310n) @@ -190,7 +189,7 @@ void Tracker::tracking(const cv::Mat & input_frame, cv::Mat & result_frame) [back to example list](#how-to-use-examples) ---- +--- ### line_follower @@ -256,7 +255,7 @@ void Follower::publish_cmdvel_for_line_following(void) This is an example for line following by RGB camera. -#### Requirements +#### Requirements - Web camera - [Logicool HD WEBCAM C310N](https://www.logicool.co.jp/ja-jp/product/hd-webcam-c310n) @@ -285,6 +284,8 @@ 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.** +**If the line detection accuracy is poor, please adjust the camera's exposure and white balance.** + #### Parameters @@ -316,7 +317,7 @@ ros2 param set /camera_follower max_brightness 80 [back to example list](#how-to-use-examples) ---- +--- ### SLAM @@ -330,13 +331,13 @@ SLAM and Navigation examples for Raspberry Pi Mouse is [here](https://github.com ### direction_controller - + This is an example to use an IMU sensor for direction control. #### Requirements -- [USB output 9 degrees IMU sensor module](https://www.rt-shop.jp/index.php?main_page=product_info&cPath=1348_1&products_id=3416&language=en) +- [USB output 9 degrees IMU sensor module](https://www.rt-shop.jp/index.php?main_page=product_info&products_id=3416&language=en) - [LiDAR Mount](https://www.rt-shop.jp/index.php?main_page=product_info&cPath=1299_1395&products_id=3867) - RT-USB-9axisIMU ROS Package. - https://github.com/rt-net/rt_usb_9axisimu_driver @@ -369,8 +370,8 @@ Then, press SW0 ~ SW2 to change the control mode as following, ### Troubleshooting -The IMU might not be connected correctly. -Reconnect the USB cable several times and re-execute the above command. +The IMU might not be connected correctly. +Reconnect the USB cable several times and re-execute the above command. #### Configure diff --git a/README.md b/README.md index fb8d1f4..7a1924f 100644 --- a/README.md +++ b/README.md @@ -14,25 +14,24 @@ Gazebo（シミュレータ）でも動作します。詳細は[こちら](https ## Supported ROS 2 distributions -- [Foxy](https://github.com/rt-net/raspimouse_ros2_examples/tree/foxy-devel) -- Humble (This branch) +- [Humble](https://github.com/rt-net/raspimouse_ros2_examples/tree/humble) +- [Jazzy](https://github.com/rt-net/raspimouse_ros2_examples/tree/jazzy) (This branch) ## Requirements - Raspberry Pi Mouse - https://rt-net.jp/products/raspberrypimousev3/ - Linux OS - - Ubuntu server 22.04 - - https://ubuntu.com/download/raspberry-pi + - Ubuntu server 24.04 - Device Driver - [rt-net/RaspberryPiMouse](https://github.com/rt-net/RaspberryPiMouse) - ROS - - [Humble Hawksbill](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) + - [Jazzy Jalisco](https://docs.ros.org/en/jazzy/index.html) - Raspberry Pi Mouse ROS 2 package - https://github.com/rt-net/raspimouse2 - Remote Computer (Optional) - ROS - - [Humble Hawksbill](https://dcs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) + - [Jazzy Jalisco](https://docs.ros.org/en/jazzy/index.html) - Raspberry Pi Mouse ROS 2 package - https://github.com/rt-net/raspimouse2 @@ -41,7 +40,7 @@ Gazebo（シミュレータ）でも動作します。詳細は[こちら](https ```sh $ cd ~/ros2_ws/src # Clone package -$ git clone -b $ROS_DISTRO-devel https://github.com/rt-net/raspimouse_ros2_examples.git +$ git clone -b $ROS_DISTRO https://github.com/rt-net/raspimouse_ros2_examples.git # Install dependencies $ rosdep install -r -y --from-paths . --ignore-src @@ -54,7 +53,7 @@ $ source ~/ros2_ws/install/setup.bash ## License -このリポジトリはApache 2.0ライセンスの元、公開されています。 +このリポジトリはApache 2.0ライセンスの元、公開されています。 ライセンスについては[LICENSE](./LICENSE)を参照ください。 ## How To Use Examples @@ -72,7 +71,7 @@ $ source ~/ros2_ws/install/setup.bash ジョイスティックコントローラでRaspberryPiMouseを動かすコード例です。 -#### Requirements +#### Requirements - Joystick Controller - [Logicool Wireless Gamepad F710](https://gaming.logicool.co.jp/ja-jp/products/gamepads/f710-wireless-gamepad.html#940-0001440) @@ -123,7 +122,7 @@ button_cmd_enable : 4 [back to example list](#how-to-use-examples) ---- +--- ### object_tracking @@ -132,7 +131,7 @@ button_cmd_enable : 4 色情報をもとにオレンジ色のボールの追跡を行うコード例です。 USB接続のWebカメラとOpenCVを使ってボール追跡をします。 -#### Requirements +#### Requirements - Webカメラ - [Logicool HD WEBCAM C310N](https://www.logicool.co.jp/ja-jp/product/hd-webcam-c310n) @@ -194,7 +193,7 @@ void Tracker::tracking(const cv::Mat & input_frame, cv::Mat & result_frame) [back to example list](#how-to-use-examples) ---- +--- ### line_follower @@ -252,7 +251,7 @@ void Follower::publish_cmdvel_for_line_following(void) [back to example list](#how-to-use-examples) ---- +--- ### camera_line_follower @@ -260,7 +259,7 @@ void Follower::publish_cmdvel_for_line_following(void) RGBカメラによるライントレースのコード例です。 -#### Requirements +#### Requirements - Webカメラ - [Logicool HD WEBCAM C310N](https://www.logicool.co.jp/ja-jp/product/hd-webcam-c310n) @@ -289,6 +288,8 @@ $ ros2 launch raspimouse_ros2_examples camera_line_follower.launch.py video_devi **画像を表示するとノードの動作が不安定になり、cmd_velや画像トピックが発行されないことがあります。** +**ラインの検出精度が悪い場合はカメラの露光やホワイトバランスの調整を行ってください。** + #### Parameters @@ -309,7 +310,7 @@ $ ros2 launch raspimouse_ros2_examples camera_line_follower.launch.py video_devi - Type: `double` - Default: 0.8 - 旋回速度の最大値 -- `area_threthold` +- `area_threshold` - Type: `double` - Default: 0.20 - 走行を開始するためのライン面積のしきい値 @@ -320,7 +321,7 @@ ros2 param set /camera_follower max_brightness 80 [back to example list](#how-to-use-examples) ---- +--- ### SLAM @@ -334,7 +335,7 @@ Raspberry Pi MouseでSLAMとNavigationを行うサンプルは[rt-net/raspimouse ### direction_controller - + IMUセンサを使用した角度制御のコード例です。 @@ -373,9 +374,9 @@ SW0 ~ SW2を押して動作モードを切り替えます。 ### Troubleshooting -IMUの接続が正常に行われない場合があります。 -その時は、IMUのUSBケーブルを抜き差ししてください。 -抜き差し実施後は、コマンドを再度実行してください。 +IMUの接続が正常に行われない場合があります。 +その時は、IMUのUSBケーブルを抜き差ししてください。 +抜き差し実施後は、コマンドを再度実行してください。 #### Configure @@ -410,7 +411,7 @@ Set parameter successful - Target angle for the SW1 control mode. - default: 0.0, min:-π, max:+π - type: double - + #### Publish topics - heading_angle - Heading angle of the robot that calculated from the IMU module sensor values. diff --git a/include/raspimouse_ros2_examples/camera_line_follower_component.hpp b/include/raspimouse_ros2_examples/camera_line_follower_component.hpp index 35e89ba..5dbcf0a 100644 --- a/include/raspimouse_ros2_examples/camera_line_follower_component.hpp +++ b/include/raspimouse_ros2_examples/camera_line_follower_component.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 RT Corporation +// Copyright 2023-2024 RT Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -19,14 +19,14 @@ #include #include "raspimouse_msgs/msg/switches.hpp" -#include "raspimouse_ros2_examples/visibility_control.h" +#include "raspimouse_ros2_examples/visibility_control.hpp" #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 "geometry_msgs/msg/twist_stamped.hpp" #include "opencv2/highgui/highgui.hpp" namespace camera_line_follower @@ -50,7 +50,8 @@ class CameraFollower : public rclcpp_lifecycle::LifecycleNode cv::Point2d object_normalized_point_; double object_normalized_area_; std::shared_ptr> result_image_pub_; - std::shared_ptr> cmd_vel_pub_; + std::shared_ptr> + cmd_vel_pub_; std::shared_ptr> motor_power_client_; rclcpp::Subscription::SharedPtr image_sub_; rclcpp::Subscription::SharedPtr switches_sub_; @@ -58,22 +59,20 @@ class CameraFollower : public rclcpp_lifecycle::LifecycleNode 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; + 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 &); + 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 diff --git a/include/raspimouse_ros2_examples/direction_controller_component.hpp b/include/raspimouse_ros2_examples/direction_controller_component.hpp index 9055aa0..d6cd802 100644 --- a/include/raspimouse_ros2_examples/direction_controller_component.hpp +++ b/include/raspimouse_ros2_examples/direction_controller_component.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 RT Corporation +// Copyright 2020-2024 RT Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -19,7 +19,7 @@ #include #include -#include "raspimouse_ros2_examples/visibility_control.h" +#include "raspimouse_ros2_examples/visibility_control.hpp" #include "raspimouse_msgs/msg/switches.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/imu.hpp" @@ -27,7 +27,7 @@ #include "std_msgs/msg/float64.hpp" #include "std_msgs/msg/string.hpp" #include "std_srvs/srv/set_bool.hpp" -#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" #include "geometry_msgs/msg/vector3.hpp" namespace direction_controller @@ -37,8 +37,7 @@ class PIDController { public: PIDController() - : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0), - error1_(0.0), error2_(0.0), output_(0.0) + : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0), error1_(0.0), error2_(0.0), output_(0.0) { } @@ -95,7 +94,7 @@ class Controller : public rclcpp::Node sensor_msgs::msg::Imu imu_data_raw_; rclcpp::Publisher::SharedPtr buzzer_pub_; - rclcpp::Publisher::SharedPtr cmd_vel_pub_; + rclcpp::Publisher::SharedPtr cmd_vel_pub_; rclcpp::Publisher::SharedPtr heading_angle_pub_; rclcpp::Subscription::SharedPtr switches_sub_; rclcpp::Subscription::SharedPtr imu_data_raw_sub_; diff --git a/include/raspimouse_ros2_examples/line_follower_component.hpp b/include/raspimouse_ros2_examples/line_follower_component.hpp index 1c3bfb7..56b03e3 100644 --- a/include/raspimouse_ros2_examples/line_follower_component.hpp +++ b/include/raspimouse_ros2_examples/line_follower_component.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 RT Corporation +// Copyright 2020-2024 RT Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -19,7 +19,7 @@ #include #include -#include "raspimouse_ros2_examples/visibility_control.h" +#include "raspimouse_ros2_examples/visibility_control.hpp" #include "raspimouse_msgs/msg/switches.hpp" #include "raspimouse_msgs/msg/light_sensors.hpp" #include "raspimouse_msgs/msg/leds.hpp" @@ -29,7 +29,7 @@ #include "std_msgs/msg/int16.hpp" #include "std_msgs/msg/string.hpp" #include "std_srvs/srv/set_bool.hpp" -#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" namespace line_follower { @@ -72,7 +72,8 @@ class Follower : public rclcpp_lifecycle::LifecycleNode bool can_publish_cmdvel_; std::shared_ptr> buzzer_pub_; - std::shared_ptr> cmd_vel_pub_; + std::shared_ptr> + cmd_vel_pub_; std::shared_ptr> leds_pub_; rclcpp::Subscription::SharedPtr light_sensors_sub_; rclcpp::Subscription::SharedPtr switches_sub_; @@ -97,16 +98,16 @@ class Follower : public rclcpp_lifecycle::LifecycleNode int median(const int value1, const int value2); void set_line_thresholds(void); - 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 &); + 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 line_follower diff --git a/include/raspimouse_ros2_examples/object_tracking_component.hpp b/include/raspimouse_ros2_examples/object_tracking_component.hpp index 7c8dc9c..8a3a052 100644 --- a/include/raspimouse_ros2_examples/object_tracking_component.hpp +++ b/include/raspimouse_ros2_examples/object_tracking_component.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 RT Corporation +// Copyright 2020-2024 RT Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -18,14 +18,14 @@ #include #include -#include "raspimouse_ros2_examples/visibility_control.h" +#include "raspimouse_ros2_examples/visibility_control.hpp" #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 "geometry_msgs/msg/twist_stamped.hpp" #include "opencv2/highgui/highgui.hpp" namespace object_tracking @@ -46,30 +46,29 @@ class Tracker : public rclcpp_lifecycle::LifecycleNode bool object_is_detected_; cv::Point2d object_normalized_point_; double object_normalized_area_; - geometry_msgs::msg::Twist cmd_vel_; + geometry_msgs::msg::TwistStamped cmd_vel_; std::shared_ptr> result_image_pub_; - std::shared_ptr> cmd_vel_pub_; + std::shared_ptr> + cmd_vel_pub_; std::shared_ptr> motor_power_client_; rclcpp::Subscription::SharedPtr image_sub_; rclcpp::TimerBase::SharedPtr cmd_vel_timer_; std::string mat_type2encoding(int mat_type); - void convert_frame_to_message( - const cv::Mat & frame, - sensor_msgs::msg::Image & msg); + void convert_frame_to_message(const cv::Mat & frame, sensor_msgs::msg::Image & msg); void tracking(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 &); + 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 object_tracking diff --git a/include/raspimouse_ros2_examples/visibility_control.h b/include/raspimouse_ros2_examples/visibility_control.hpp similarity index 89% rename from include/raspimouse_ros2_examples/visibility_control.h rename to include/raspimouse_ros2_examples/visibility_control.hpp index 40d4c42..e9df852 100644 --- a/include/raspimouse_ros2_examples/visibility_control.h +++ b/include/raspimouse_ros2_examples/visibility_control.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 RT Corporation +// Copyright 2020-2024 RT Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RASPIMOUSE_ROS2_EXAMPLES__VISIBILITY_CONTROL_H_ -#define RASPIMOUSE_ROS2_EXAMPLES__VISIBILITY_CONTROL_H_ +#ifndef RASPIMOUSE_ROS2_EXAMPLES__VISIBILITY_CONTROL_HPP_ +#define RASPIMOUSE_ROS2_EXAMPLES__VISIBILITY_CONTROL_HPP_ #ifdef __cplusplus extern "C" @@ -55,4 +55,4 @@ extern "C" } #endif -#endif // RASPIMOUSE_ROS2_EXAMPLES__VISIBILITY_CONTROL_H_ +#endif // RASPIMOUSE_ROS2_EXAMPLES__VISIBILITY_CONTROL_HPP_ diff --git a/launch/camera_line_follower.launch.py b/launch/camera_line_follower.launch.py index 2424bc1..292c575 100644 --- a/launch/camera_line_follower.launch.py +++ b/launch/camera_line_follower.launch.py @@ -1,4 +1,4 @@ -# Copyright 2023 RT Corporation +# Copyright 2023-2024 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -14,8 +14,8 @@ import launch from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode @@ -23,58 +23,55 @@ def generate_launch_description(): declare_mouse = DeclareLaunchArgument( - 'mouse', - default_value="true", - description='Launch raspimouse node' + 'mouse', default_value='true', description='Launch raspimouse node' ) declare_use_camera_node = DeclareLaunchArgument( - 'use_camera_node', - default_value='true', - description='Use camera node.' + 'use_camera_node', default_value='true', description='Use camera node.' ) declare_video_device = DeclareLaunchArgument( - 'video_device', - default_value='/dev/video0', - description='Set video device.' + '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', + 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( @@ -82,13 +79,15 @@ def generate_launch_description(): package='raspimouse_ros2_examples', executable='lifecycle_node_manager', output='screen', - parameters=[{'components': ['raspimouse', 'camera_follower']}] + parameters=[{'components': ['raspimouse', 'camera_follower']}], ) - return launch.LaunchDescription([ - declare_mouse, - declare_use_camera_node, - declare_video_device, - container, - manager - ]) + return launch.LaunchDescription( + [ + declare_mouse, + declare_use_camera_node, + declare_video_device, + container, + manager, + ] + ) diff --git a/launch/direction_controller.launch.py b/launch/direction_controller.launch.py index 72f3b43..3ba8ccd 100644 --- a/launch/direction_controller.launch.py +++ b/launch/direction_controller.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2020-2024 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -13,26 +13,36 @@ # limitations under the License. import launch -from launch_ros.actions import Node from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node def generate_launch_description(): """Generate launch description with multiple components.""" mouse_node = LifecycleNode( - name='raspimouse', namespace="", + name='raspimouse', + namespace='', package='raspimouse', executable='raspimouse', output='screen', - parameters=[{'use_light_sensors': False, }] + parameters=[ + { + 'use_light_sensors': False, + } + ], ) imu_driver = LifecycleNode( - name='rt_usb_9axisimu_driver', namespace="", + name='rt_usb_9axisimu_driver', + namespace='', package='rt_usb_9axisimu_driver', executable='rt_usb_9axisimu_driver', output='screen', - parameters=[{'port': '/dev/ttyACM0', }] + parameters=[ + { + 'port': '/dev/ttyACM0', + } + ], ) direction_controller = Node( @@ -46,8 +56,9 @@ def generate_launch_description(): package='raspimouse_ros2_examples', executable='lifecycle_node_manager', output='screen', - parameters=[{'components': ['raspimouse', 'rt_usb_9axisimu_driver']}] + parameters=[{'components': ['raspimouse', 'rt_usb_9axisimu_driver']}], ) - return launch.LaunchDescription([ - mouse_node, imu_driver, direction_controller, manager]) + return launch.LaunchDescription( + [mouse_node, imu_driver, direction_controller, manager] + ) diff --git a/launch/line_follower.launch.py b/launch/line_follower.launch.py index bf09658..d338b17 100644 --- a/launch/line_follower.launch.py +++ b/launch/line_follower.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2020-2024 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -22,7 +22,7 @@ def generate_launch_description(): package='raspimouse_ros2_examples', executable='line_follower', output='screen', - parameters=[{'use_pulse_counters': False}] + parameters=[{'use_pulse_counters': False}], ) manager = Node( @@ -30,7 +30,7 @@ def generate_launch_description(): package='raspimouse_ros2_examples', executable='lifecycle_node_manager', output='screen', - parameters=[{'components': ['raspimouse', 'follower']}] + parameters=[{'components': ['raspimouse', 'follower']}], ) return launch.LaunchDescription([line_follower, manager]) diff --git a/launch/mouse_with_lidar.launch.py b/launch/mouse_with_lidar.launch.py index d0ce778..7487b8d 100644 --- a/launch/mouse_with_lidar.launch.py +++ b/launch/mouse_with_lidar.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2020-2024 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -25,25 +25,40 @@ def generate_launch_description(): declare_lidar = DeclareLaunchArgument( - 'lidar', default_value='lds', - description='LiDAR: lds only, for now.' + 'lidar', default_value='lds', description='LiDAR: lds only, for now.' ) mouse_node = LifecycleNode( - name='raspimouse', namespace="", - package='raspimouse', executable='raspimouse', output='screen', - parameters=[os.path.join(get_package_share_directory( - 'raspimouse_ros2_examples'), 'config', 'mouse.yml')] + name='raspimouse', + namespace='', + package='raspimouse', + executable='raspimouse', + output='screen', + parameters=[ + os.path.join( + get_package_share_directory('raspimouse_ros2_examples'), + 'config', + 'mouse.yml', ) + ], + ) def func_launch_lidar_node(context): if context.launch_configurations['lidar'] == 'lds': - return [IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('hls_lfcd_lds_driver'), - 'launch'), - '/hlds_laser.launch.py' - ]),)] + return [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join( + get_package_share_directory('hls_lfcd_lds_driver'), + 'launch', + ), + '/hlds_laser.launch.py', + ] + ), + ) + ] + launch_lidar_node = OpaqueFunction(function=func_launch_lidar_node) ld = LaunchDescription() diff --git a/launch/object_tracking.launch.py b/launch/object_tracking.launch.py index e29a51a..f9ee2ea 100644 --- a/launch/object_tracking.launch.py +++ b/launch/object_tracking.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2020-2024 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -14,8 +14,8 @@ import launch from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode @@ -23,54 +23,51 @@ def generate_launch_description(): declare_mouse = DeclareLaunchArgument( - 'mouse', - default_value="true", - description='Launch raspimouse node' + 'mouse', default_value='true', description='Launch raspimouse node' ) declare_use_camera_node = DeclareLaunchArgument( - 'use_camera_node', - default_value='true', - description='Use camera node.' + 'use_camera_node', default_value='true', description='Use camera node.' ) declare_video_device = DeclareLaunchArgument( - 'video_device', - default_value='/dev/video0', - description='Set video device.' + 'video_device', default_value='/dev/video0', description='Set video device.' ) """Generate launch description with multiple components.""" container = ComposableNodeContainer( - name='object_tracking_container', - namespace='', - package='rclcpp_components', - executable='component_container_mt', - composable_node_descriptions=[ - ComposableNode( - package='raspimouse_ros2_examples', - plugin='object_tracking::Tracker', - name='tracker', - 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', + name='object_tracking_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + ComposableNode( + package='raspimouse_ros2_examples', + plugin='object_tracking::Tracker', + name='tracker', + 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( @@ -78,13 +75,15 @@ def generate_launch_description(): package='raspimouse_ros2_examples', executable='lifecycle_node_manager', output='screen', - parameters=[{'components': ['raspimouse', 'tracker']}] + parameters=[{'components': ['raspimouse', 'tracker']}], ) - return launch.LaunchDescription([ - declare_mouse, - declare_use_camera_node, - declare_video_device, - container, - manager - ]) + return launch.LaunchDescription( + [ + declare_mouse, + declare_use_camera_node, + declare_video_device, + container, + manager, + ] + ) diff --git a/launch/slam.launch.py b/launch/slam.launch.py index 35ad664..3440fe3 100644 --- a/launch/slam.launch.py +++ b/launch/slam.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2020-2024 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -20,29 +20,32 @@ def generate_launch_description(): slam_node = Node( - package='slam_toolbox', executable='sync_slam_toolbox_node', + package='slam_toolbox', + executable='sync_slam_toolbox_node', output='screen', parameters=[ - get_package_share_directory( - 'raspimouse_ros2_examples') + get_package_share_directory('raspimouse_ros2_examples') + '/config/mapper_params_offline.yaml' ], ) rviz2_node = Node( name='rviz2', - package='rviz2', executable='rviz2', output='screen', + package='rviz2', + executable='rviz2', + output='screen', arguments=[ '-d', get_package_share_directory('raspimouse_ros2_examples') - + '/config/default.rviz'], + + '/config/default.rviz', + ], ) static_transform_publisher_node = Node( package='tf2_ros', - executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0.1', '0', '3.14', - '3.14', 'base_footprint', 'laser'], + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0.1', '0', '3.14', '3.14', 'base_footprint', 'laser'], ) ld = LaunchDescription() diff --git a/launch/teleop_joy.launch.py b/launch/teleop_joy.launch.py index 210b814..67aa33e 100644 --- a/launch/teleop_joy.launch.py +++ b/launch/teleop_joy.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2020-2024 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -21,46 +21,46 @@ from launch.actions import LogInfo from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration +from launch.events import Shutdown from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node from launch_ros.actions import LifecycleNode -from launch.events import Shutdown +from launch_ros.actions import Node def generate_launch_description(): - joydev = LaunchConfiguration('joydev') declare_joydev = DeclareLaunchArgument( - 'joydev', default_value='/dev/input/js0', - description='Device file for JoyStick Controller' + 'joydev', + default_value='/dev/input/js0', + description='Device file for JoyStick Controller', ) declare_joyconfig = DeclareLaunchArgument( - 'joyconfig', default_value='f710', - description='Keyconfig of joystick controllers: supported: f710, dualshock3' + 'joyconfig', + default_value='f710', + description='Keyconfig of joystick controllers: supported: f710, dualshock3', ) declare_mouse = DeclareLaunchArgument( - 'mouse', default_value="true", - description='Launch raspimouse node' + 'mouse', default_value='true', description='Launch raspimouse node' ) def func_get_joyconfig_file_name(context): param_file = os.path.join( get_package_share_directory('raspimouse_ros2_examples'), 'config', - 'joy_' + context.launch_configurations['joyconfig'] + '.yml') + 'joy_' + context.launch_configurations['joyconfig'] + '.yml', + ) if os.path.exists(param_file): return [SetLaunchConfiguration('joyconfig_filename', param_file)] else: - return [LogInfo(msg=param_file + " is not exist.")] + return [LogInfo(msg=param_file + ' is not exist.')] + get_joyconfig_file_name = OpaqueFunction(function=func_get_joyconfig_file_name) joy_node = Node( - package='joy_linux', - executable='joy_linux_node', - parameters=[{'dev': joydev}] + package='joy_linux', executable='joy_linux_node', parameters=[{'dev': joydev}] ) joystick_control_node = Node( @@ -71,13 +71,24 @@ def func_get_joyconfig_file_name(context): ) def func_launch_mouse_node(context): - if context.launch_configurations['mouse'] == "true": - return [LifecycleNode( - name='raspimouse', namespace="", - package='raspimouse', executable='raspimouse', output='screen', - parameters=[os.path.join(get_package_share_directory( - 'raspimouse_ros2_examples'), 'config', 'mouse.yml')] - )] + if context.launch_configurations['mouse'] == 'true': + return [ + LifecycleNode( + name='raspimouse', + namespace='', + package='raspimouse', + executable='raspimouse', + output='screen', + parameters=[ + os.path.join( + get_package_share_directory('raspimouse_ros2_examples'), + 'config', + 'mouse.yml', + ) + ], + ) + ] + mouse_node = OpaqueFunction(function=func_launch_mouse_node) ld = LaunchDescription() diff --git a/package.xml b/package.xml index 5169dce..3ed617c 100644 --- a/package.xml +++ b/package.xml @@ -11,6 +11,8 @@ ShotaAk Daisuke Sato Shuhei Kozasa + Yusuke Kato + Kazushi Kurasawa ament_cmake diff --git a/raspimouse_ros2_examples/joystick_control.py b/raspimouse_ros2_examples/joystick_control.py index 892c13a..66e4f31 100755 --- a/raspimouse_ros2_examples/joystick_control.py +++ b/raspimouse_ros2_examples/joystick_control.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # coding: UTF-8 -# Copyright 2020 RT Corporation +# Copyright 2020-2024 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -19,7 +19,7 @@ import math from time import sleep -from geometry_msgs.msg import Twist +from geometry_msgs.msg import TwistStamped from lifecycle_msgs.msg import Transition from lifecycle_msgs.srv import ChangeState from lifecycle_msgs.srv import GetState @@ -28,15 +28,14 @@ from raspimouse_msgs.msg import Switches import rclpy -from rclpy.node import Node from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.node import Node from sensor_msgs.msg import Joy from std_msgs.msg import Int16 from std_srvs.srv import SetBool class JoyWrapper(Node): - def __init__(self): super().__init__('joystick_control') @@ -97,7 +96,9 @@ def __init__(self): self._BUTTON_BUZZER6 = self.get_parameter('button_buzzer6').value self._BUTTON_BUZZER7 = self.get_parameter('button_buzzer7').value - self._BUTTON_SENSOR_SOUND_EN = self.get_parameter('button_sensor_sound_en').value + self._BUTTON_SENSOR_SOUND_EN = self.get_parameter( + 'button_sensor_sound_en' + ).value self._BUTTON_CONFIG_ENABLE = self.get_parameter('button_config_enable').value # for _joy_velocity_config() @@ -117,7 +118,7 @@ def __init__(self): self._node_logger = self.get_logger() - self._pub_cmdvel = self.create_publisher(Twist, 'cmd_vel', 1) + self._pub_cmdvel = self.create_publisher(TwistStamped, 'cmd_vel', 1) self._pub_buzzer = self.create_publisher(Int16, 'buzzer', 1) self._pub_leds = self.create_publisher(Leds, 'leds', 1) @@ -125,37 +126,53 @@ def __init__(self): self._client_cb_group = MutuallyExclusiveCallbackGroup() self._client_get_state = self.create_client( - GetState, 'raspimouse/get_state', callback_group=self._client_cb_group) + GetState, 'raspimouse/get_state', callback_group=self._client_cb_group + ) while not self._client_get_state.wait_for_service(timeout_sec=1.0): - self._node_logger.warn(self._client_get_state.srv_name + ' service not available') + self._node_logger.warn( + self._client_get_state.srv_name + ' service not available' + ) self._client_change_state = self.create_client( - ChangeState, 'raspimouse/change_state', callback_group=self._client_cb_group) + ChangeState, 'raspimouse/change_state', callback_group=self._client_cb_group + ) while not self._client_change_state.wait_for_service(timeout_sec=1.0): - self._node_logger.warn(self._client_change_state.srv_name + ' service not available') + self._node_logger.warn( + self._client_change_state.srv_name + ' service not available' + ) self._activate_raspimouse() self._client_motor_power = self.create_client( - SetBool, 'motor_power', callback_group=self._client_cb_group) + SetBool, 'motor_power', callback_group=self._client_cb_group + ) while not self._client_motor_power.wait_for_service(timeout_sec=1.0): - self._node_logger.warn(self._client_motor_power.srv_name + ' service not available') + self._node_logger.warn( + self._client_motor_power.srv_name + ' service not available' + ) self._motor_on() self._sub_joy = self.create_subscription( - Joy, 'joy', self._callback_joy, 1, - callback_group=self._sub_cb_group) + Joy, 'joy', self._callback_joy, 1, callback_group=self._sub_cb_group + ) self._sub_lightsensor = self.create_subscription( - LightSensors, 'light_sensors', self._callback_lightsensors, 1, - callback_group=self._sub_cb_group) + LightSensors, + 'light_sensors', + self._callback_lightsensors, + 1, + callback_group=self._sub_cb_group, + ) self._sub_switches = self.create_subscription( - Switches, 'switches', self._callback_switches, 1, - callback_group=self._sub_cb_group) + Switches, + 'switches', + self._callback_switches, + 1, + callback_group=self._sub_cb_group, + ) def _activate_raspimouse(self): self._set_mouse_lifecycle_state(Transition.TRANSITION_CONFIGURE) self._set_mouse_lifecycle_state(Transition.TRANSITION_ACTIVATE) - self._node_logger.info('Mouse state is ' - + self._get_mouse_lifecycle_state()) + self._node_logger.info('Mouse state is ' + self._get_mouse_lifecycle_state()) def _set_mouse_lifecycle_state(self, transition_id): request = ChangeState.Request() @@ -196,8 +213,10 @@ def _callback_switches(self, msg): self._mouse_switches = msg def _joy_shutdown(self, joy_msg): - if joy_msg.buttons[self._BUTTON_SHUTDOWN_1] and\ - joy_msg.buttons[self._BUTTON_SHUTDOWN_2]: + if ( + joy_msg.buttons[self._BUTTON_SHUTDOWN_1] + and joy_msg.buttons[self._BUTTON_SHUTDOWN_2] + ): self._pub_leds.publish(Leds()) self._motor_off() self._set_mouse_lifecycle_state(Transition.TRANSITION_DEACTIVATE) @@ -212,13 +231,20 @@ def _joy_motor_onoff(self, joy_msg): self._motor_off() def _joy_cmdvel(self, joy_msg): - cmdvel = Twist() + cmdvel = TwistStamped() if joy_msg.buttons[self._BUTTON_CMD_ENABLE]: - cmdvel.linear.x = self._vel_linear_x * joy_msg.axes[self._AXIS_CMD_LINEAR_X] - cmdvel.angular.z = self._vel_angular_z * joy_msg.axes[self._AXIS_CMD_ANGULAR_Z] + cmdvel.twist.linear.x = ( + self._vel_linear_x * joy_msg.axes[self._AXIS_CMD_LINEAR_X] + ) + cmdvel.twist.angular.z = ( + self._vel_angular_z * joy_msg.axes[self._AXIS_CMD_ANGULAR_Z] + ) self._node_logger.info( - 'linear_x:' + str(cmdvel.linear.x) + - ', angular_z:' + str(cmdvel.angular.z)) + 'linear_x:' + + str(cmdvel.twist.linear.x) + + ', angular_z:' + + str(cmdvel.twist.angular.z) + ) self._pub_cmdvel.publish(cmdvel) self._cmdvel_has_value = True @@ -277,20 +303,17 @@ def _beep_buzzer(self, freq_data, beep_time=0): def _joy_buzzer_freq(self, joy_msg): freq = Int16() buttons = [ - self._dpad(joy_msg, self._DPAD_BUZZER0), - self._dpad(joy_msg, self._DPAD_BUZZER1), - self._dpad(joy_msg, self._DPAD_BUZZER2), - self._dpad(joy_msg, self._DPAD_BUZZER3), - joy_msg.buttons[self._BUTTON_BUZZER4], - joy_msg.buttons[self._BUTTON_BUZZER5], - joy_msg.buttons[self._BUTTON_BUZZER6], - joy_msg.buttons[self._BUTTON_BUZZER7], - ] + self._dpad(joy_msg, self._DPAD_BUZZER0), + self._dpad(joy_msg, self._DPAD_BUZZER1), + self._dpad(joy_msg, self._DPAD_BUZZER2), + self._dpad(joy_msg, self._DPAD_BUZZER3), + joy_msg.buttons[self._BUTTON_BUZZER4], + joy_msg.buttons[self._BUTTON_BUZZER5], + joy_msg.buttons[self._BUTTON_BUZZER6], + joy_msg.buttons[self._BUTTON_BUZZER7], + ] # buzzer frequency Hz - SCALES = [ - 523, 587, 659, 699, - 784, 880, 987, 1046 - ] + SCALES = [523, 587, 659, 699, 784, 880, 987, 1046] if joy_msg.buttons[self._BUTTON_BUZZER_ENABLE]: for i, button in enumerate(buttons): @@ -338,28 +361,38 @@ def _joy_velocity_config(self, joy_msg): if joy_msg.buttons[self._BUTTON_CONFIG_ENABLE]: any_switch_pressed = False - if any([self._mouse_switches.switch0, + if any( + [ + self._mouse_switches.switch0, self._mouse_switches.switch1, - self._mouse_switches.switch2]): + self._mouse_switches.switch2, + ] + ): any_switch_pressed = True if not self._switch_has_been_pressed: if self._mouse_switches.switch0: self._vel_linear_x = self._config_velocity( - self._vel_linear_x, ADD_VEL_LINEAR_X, - 0, self._MAX_VEL_LINEAR_X) + self._vel_linear_x, ADD_VEL_LINEAR_X, 0, self._MAX_VEL_LINEAR_X + ) self._vel_angular_z = self._config_velocity( - self._vel_angular_z, ADD_VEL_ANGULAR_Z, - 0, self._MAX_VEL_ANGULAR_Z) + self._vel_angular_z, + ADD_VEL_ANGULAR_Z, + 0, + self._MAX_VEL_ANGULAR_Z, + ) self._beep_buzzer(BUZZER_FREQ_ADD, BUZZER_BEEP_TIME) elif self._mouse_switches.switch2: self._vel_linear_x = self._config_velocity( - self._vel_linear_x, -ADD_VEL_LINEAR_X, - 0, self._MAX_VEL_LINEAR_X) + self._vel_linear_x, -ADD_VEL_LINEAR_X, 0, self._MAX_VEL_LINEAR_X + ) self._vel_angular_z = self._config_velocity( - self._vel_angular_z, -ADD_VEL_ANGULAR_Z, - 0, self._MAX_VEL_ANGULAR_Z) + self._vel_angular_z, + -ADD_VEL_ANGULAR_Z, + 0, + self._MAX_VEL_ANGULAR_Z, + ) self._beep_buzzer(BUZZER_FREQ_SUB, BUZZER_BEEP_TIME) elif self._mouse_switches.switch1: @@ -369,9 +402,11 @@ def _joy_velocity_config(self, joy_msg): self._switch_has_been_pressed = any_switch_pressed self._node_logger.info( - 'linear_x:' + str(self._vel_linear_x) + - ', angular_z:' + str(self._vel_angular_z) - ) + 'linear_x:' + + str(self._vel_linear_x) + + ', angular_z:' + + str(self._vel_angular_z) + ) def _config_velocity(self, current, add, lowerlimit, upperlimit): output = current + add @@ -409,7 +444,9 @@ def main(args=None): try: rclpy.spin(joy_wrapper) except SystemExit: - rclpy.logging.get_logger("joystick_control").info('_joy_shutdown() has been executed') + rclpy.logging.get_logger('joystick_control').info( + '_joy_shutdown() has been executed' + ) joy_wrapper.destroy_node() diff --git a/src/camera_line_follower_component.cpp b/src/camera_line_follower_component.cpp index 2acc8b4..5990341 100644 --- a/src/camera_line_follower_component.cpp +++ b/src/camera_line_follower_component.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 RT Corporation +// Copyright 2023-2024 RT Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -24,9 +24,9 @@ #include #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" -#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" #include "lifecycle_msgs/srv/change_state.hpp" -#include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" constexpr auto MIN_BRIGHTNESS_PARAM = "min_brightness"; constexpr auto MAX_BRIGHTNESS_PARAM = "max_brightness"; @@ -41,8 +41,7 @@ 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) +: rclcpp_lifecycle::LifecycleNode("camera_follower", options), object_is_detected_(false) { } @@ -76,35 +75,35 @@ void CameraFollower::callback_switches(const raspimouse_msgs::msg::Switches::Sha void CameraFollower::on_cmd_vel_timer() { - geometry_msgs::msg::Twist cmd_vel; + geometry_msgs::msg::TwistStamped cmd_vel; // Follow the line // when the number of pixels of the object is greater than the threshold. - if (object_is_detected_ && + 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; + cmd_vel.twist.linear.x = get_parameter(LINEAR_VEL_PARAM).as_double(); + cmd_vel.twist.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; + cmd_vel.twist.linear.x = 0.0; + cmd_vel.twist.angular.z = 0.0; } if (!enable_following_) { - cmd_vel.linear.x = 0.0; - cmd_vel.angular.z = 0.0; + cmd_vel.twist.linear.x = 0.0; + cmd_vel.twist.angular.z = 0.0; } - auto msg = std::make_unique(cmd_vel); + 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."); + RCLCPP_ERROR(this->get_logger(), "Service motor_power is not avaliable."); return; } auto request = std::make_shared(); @@ -151,10 +150,8 @@ bool CameraFollower::detect_line(const cv::Mat & input_frame, cv::Mat & result_f 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); + 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 @@ -184,20 +181,17 @@ bool CameraFollower::detect_line(const cv::Mat & input_frame, cv::Mat & result_f // 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 - ); + 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); + 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); + result_frame, text, cv::Point(0, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255), + 2); return true; } else { return false; @@ -214,14 +208,12 @@ CallbackReturn CameraFollower::on_configure(const rclcpp_lifecycle::State &) 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."); + 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); + 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)); diff --git a/src/direction_controller.cpp b/src/direction_controller.cpp index 4806cf6..f304cb1 100644 --- a/src/direction_controller.cpp +++ b/src/direction_controller.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 RT Corporation +// Copyright 2020-2024 RT Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/src/direction_controller_component.cpp b/src/direction_controller_component.cpp index 34bd398..c0a335a 100644 --- a/src/direction_controller_component.cpp +++ b/src/direction_controller_component.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 RT Corporation +// Copyright 2020-2024 RT Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -42,7 +42,7 @@ Controller::Controller(const rclcpp::NodeOptions & options) cmd_vel_timer_ = create_wall_timer(16ms, std::bind(&Controller::on_cmd_vel_timer, this)); - cmd_vel_pub_ = create_publisher("cmd_vel", 1); + cmd_vel_pub_ = create_publisher("cmd_vel", 1); buzzer_pub_ = create_publisher("buzzer", 1); heading_angle_pub_ = create_publisher("heading_angle", 1); switches_sub_ = create_subscription( @@ -73,8 +73,7 @@ Controller::Controller(const rclcpp::NodeOptions & options) this->declare_parameter("d_gain", 20.0, descriptor); omega_pid_controller_.set_gain( - this->get_parameter("p_gain").as_double(), - this->get_parameter("i_gain").as_double(), + this->get_parameter("p_gain").as_double(), this->get_parameter("i_gain").as_double(), this->get_parameter("d_gain").as_double()); pressed_switch_number_ = -1; @@ -104,8 +103,7 @@ void Controller::on_cmd_vel_timer() } omega_pid_controller_.set_gain( - this->get_parameter("p_gain").as_double(), - this->get_parameter("i_gain").as_double(), + this->get_parameter("p_gain").as_double(), this->get_parameter("i_gain").as_double(), this->get_parameter("d_gain").as_double()); if (released_switch_number != -1 || filtered_acc_.z > 0.0) { @@ -170,9 +168,7 @@ void Controller::callback_imu_data_raw(const sensor_msgs::msg::Imu::SharedPtr ms bool Controller::set_motor_power(const bool motor_on) { if (!motor_power_client_->wait_for_service(5s)) { - RCLCPP_ERROR( - this->get_logger(), - "Service motor_power is not avaliable."); + RCLCPP_ERROR(this->get_logger(), "Service motor_power is not avaliable."); return false; } auto request = std::make_shared(); @@ -189,9 +185,8 @@ bool Controller::omega_calibration(const double omega) omega_samples_.push_back(omega); if (omega_samples_.size() >= SAMPLE_NUM) { - omega_bias_ = std::accumulate( - std::begin(omega_samples_), - std::end(omega_samples_), 0.0) / omega_samples_.size(); + omega_bias_ = std::accumulate(std::begin(omega_samples_), std::end(omega_samples_), 0.0) / + omega_samples_.size(); omega_samples_.clear(); complete = true; } @@ -225,8 +220,8 @@ void Controller::angle_control(const double target_angle) { const double SIGN = 1.0; - auto cmd_vel = std::make_unique(); - cmd_vel->angular.z = SIGN * omega_pid_controller_.update(heading_angle_, target_angle); + auto cmd_vel = std::make_unique(); + cmd_vel->twist.angular.z = SIGN * omega_pid_controller_.update(heading_angle_, target_angle); cmd_vel_pub_->publish(std::move(cmd_vel)); } @@ -267,10 +262,7 @@ void Controller::beep_buzzer(const int freq, const std::chrono::nanoseconds & be buzzer_pub_->publish(std::move(msg)); } -void Controller::beep_start(void) -{ - beep_buzzer(1000, 500ms); -} +void Controller::beep_start(void) {beep_buzzer(1000, 500ms);} void Controller::beep_success(void) { @@ -287,7 +279,6 @@ void Controller::beep_failure(void) } } - } // namespace direction_controller #include "rclcpp_components/register_node_macro.hpp" diff --git a/src/lifecycle_node_manager.cpp b/src/lifecycle_node_manager.cpp index 4907037..f9fdf8a 100644 --- a/src/lifecycle_node_manager.cpp +++ b/src/lifecycle_node_manager.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 RT Corporation +// Copyright 2020-2024 RT Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -33,17 +33,14 @@ using SrvGetState = lifecycle_msgs::srv::GetState; using SrvChangeState = lifecycle_msgs::srv::ChangeState; std::uint8_t state_of( - std::string target_node_name, - rclcpp::Node::SharedPtr node, std::chrono::seconds time_out = 10s) + std::string target_node_name, rclcpp::Node::SharedPtr node, std::chrono::seconds time_out = 10s) { auto request = std::make_shared(); auto service_name = target_node_name + "/get_state"; auto client = node->create_client(service_name); if (!client->wait_for_service(time_out)) { - RCLCPP_ERROR( - node->get_logger(), - "Service %s is not avaliable.", service_name.c_str()); + RCLCPP_ERROR(node->get_logger(), "Service %s is not avaliable.", service_name.c_str()); return MsgState::PRIMARY_STATE_UNKNOWN; } @@ -52,8 +49,7 @@ std::uint8_t state_of( if (future_status != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR( - node->get_logger(), - "Service %s time out while getting current state.", service_name.c_str()); + node->get_logger(), "Service %s time out while getting current state.", service_name.c_str()); return MsgState::PRIMARY_STATE_UNKNOWN; } @@ -61,41 +57,32 @@ std::uint8_t state_of( } bool all_nodes_are_unconfigured( - rclcpp::Node::SharedPtr node, - const std::vector & target_node_names) + rclcpp::Node::SharedPtr node, const std::vector & target_node_names) { - return std::all_of( - target_node_names.begin(), target_node_names.end(), - [&](std::string s) { - return state_of(s, node, 10s) == MsgState::PRIMARY_STATE_UNCONFIGURED; - }); + return std::all_of(target_node_names.begin(), target_node_names.end(), [&](std::string s) { + return state_of(s, node, 10s) == MsgState::PRIMARY_STATE_UNCONFIGURED; + }); } bool all_nodes_are_inactive( - rclcpp::Node::SharedPtr node, - const std::vector & target_node_names) + rclcpp::Node::SharedPtr node, const std::vector & target_node_names) { - return std::all_of( - target_node_names.begin(), target_node_names.end(), - [&](std::string s) { - return state_of(s, node, 10s) == MsgState::PRIMARY_STATE_INACTIVE; - }); + return std::all_of(target_node_names.begin(), target_node_names.end(), [&](std::string s) { + return state_of(s, node, 10s) == MsgState::PRIMARY_STATE_INACTIVE; + }); } bool all_nodes_are_active( - rclcpp::Node::SharedPtr node, - const std::vector & target_node_names) + rclcpp::Node::SharedPtr node, const std::vector & target_node_names) { - return std::all_of( - target_node_names.begin(), target_node_names.end(), - [&](std::string s) { - return state_of(s, node, 10s) == MsgState::PRIMARY_STATE_ACTIVE; - }); + return std::all_of(target_node_names.begin(), target_node_names.end(), [&](std::string s) { + return state_of(s, node, 10s) == MsgState::PRIMARY_STATE_ACTIVE; + }); } bool change_state( - std::string target_node_name, rclcpp::Node::SharedPtr node, - std::uint8_t transition, std::chrono::seconds time_out = 10s) + std::string target_node_name, rclcpp::Node::SharedPtr node, std::uint8_t transition, + std::chrono::seconds time_out = 10s) { auto request = std::make_shared(); request->transition.id = transition; @@ -104,9 +91,7 @@ bool change_state( auto client = node->create_client(service_name); if (!client->wait_for_service(time_out)) { - RCLCPP_ERROR( - node->get_logger(), - "Service %s is not avaliable.", service_name.c_str()); + RCLCPP_ERROR(node->get_logger(), "Service %s is not avaliable.", service_name.c_str()); return false; } @@ -115,8 +100,8 @@ bool change_state( if (future_status != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR( - node->get_logger(), - "Service %s time out while changing current state.", service_name.c_str()); + node->get_logger(), "Service %s time out while changing current state.", + service_name.c_str()); return false; } @@ -124,28 +109,21 @@ bool change_state( } bool configure_all_nodes( - rclcpp::Node::SharedPtr node, - const std::vector & target_node_names) + rclcpp::Node::SharedPtr node, const std::vector & target_node_names) { - return std::all_of( - target_node_names.begin(), target_node_names.end(), - [&](std::string s) { - return change_state(s, node, MsgTransition::TRANSITION_CONFIGURE, 10s); - }); + return std::all_of(target_node_names.begin(), target_node_names.end(), [&](std::string s) { + return change_state(s, node, MsgTransition::TRANSITION_CONFIGURE, 10s); + }); } bool activate_all_nodes( - rclcpp::Node::SharedPtr node, - const std::vector & target_node_names) + rclcpp::Node::SharedPtr node, const std::vector & target_node_names) { - return std::all_of( - target_node_names.begin(), target_node_names.end(), - [&](std::string s) { - return change_state(s, node, MsgTransition::TRANSITION_ACTIVATE, 10s); - }); + return std::all_of(target_node_names.begin(), target_node_names.end(), [&](std::string s) { + return change_state(s, node, MsgTransition::TRANSITION_ACTIVATE, 10s); + }); } - int main(int argc, char * argv[]) { // Force flush of the stdout buffer. diff --git a/src/line_follower.cpp b/src/line_follower.cpp index 1392631..26b2f89 100644 --- a/src/line_follower.cpp +++ b/src/line_follower.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 RT Corporation +// Copyright 2020-2024 RT Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/src/line_follower_component.cpp b/src/line_follower_component.cpp index 2f5705f..d4ca982 100644 --- a/src/line_follower_component.cpp +++ b/src/line_follower_component.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 RT Corporation +// Copyright 2020-2024 RT Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -25,7 +25,6 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" -#include "lifecycle_msgs/srv/change_state.hpp" using namespace std::chrono_literals; using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -44,8 +43,10 @@ Follower::Follower(const rclcpp::NodeOptions & options) sampling_values_(SensorsType(SENSOR_NUM, 0)), line_is_detected_by_sensor_(std::vector(SENSOR_NUM, false)), sampling_count_(0), - line_values_are_sampled_(false), field_values_are_sampled_(false), - line_sampling_(false), field_sampling_(false), + line_values_are_sampled_(false), + field_values_are_sampled_(false), + line_sampling_(false), + field_sampling_(false), can_publish_cmdvel_(false) { } @@ -118,11 +119,11 @@ void Follower::set_motor_power(const bool motor_on) void Follower::publish_cmdvel_for_line_following(void) { - const double VEL_LINEAR_X = 0.08; // m/s - const double VEL_ANGULAR_Z = 0.8; // rad/s + const double VEL_LINEAR_X = 0.08; // m/s + const double VEL_ANGULAR_Z = 0.8; // rad/s const double LOW_VEL_ANGULAR_Z = 0.5; // rad/s - auto cmd_vel = std::make_unique(); + auto cmd_vel = std::make_unique(); bool detect_line = std::any_of( line_is_detected_by_sensor_.begin(), line_is_detected_by_sensor_.end(), @@ -132,22 +133,22 @@ void Follower::publish_cmdvel_for_line_following(void) [](bool detected) {return !detected;}); if (detect_line && detect_field) { - cmd_vel->linear.x = VEL_LINEAR_X; + cmd_vel->twist.linear.x = VEL_LINEAR_X; if (line_is_detected_by_sensor_[LEFT]) { - cmd_vel->angular.z += VEL_ANGULAR_Z; + cmd_vel->twist.angular.z += VEL_ANGULAR_Z; } if (line_is_detected_by_sensor_[RIGHT]) { - cmd_vel->angular.z -= VEL_ANGULAR_Z; + cmd_vel->twist.angular.z -= VEL_ANGULAR_Z; } if (line_is_detected_by_sensor_[MID_LEFT]) { - cmd_vel->angular.z += LOW_VEL_ANGULAR_Z; + cmd_vel->twist.angular.z += LOW_VEL_ANGULAR_Z; } if (line_is_detected_by_sensor_[MID_RIGHT]) { - cmd_vel->angular.z -= LOW_VEL_ANGULAR_Z; + cmd_vel->twist.angular.z -= LOW_VEL_ANGULAR_Z; } } @@ -200,10 +201,7 @@ void Follower::beep_buzzer(const int freq, const std::chrono::nanoseconds & beep buzzer_pub_->publish(std::move(msg)); } -void Follower::beep_start(void) -{ - beep_buzzer(1000, 500ms); -} +void Follower::beep_start(void) {beep_buzzer(1000, 500ms);} void Follower::beep_success(void) { @@ -254,9 +252,8 @@ void Follower::multisampling(void) line_sampling_ = field_sampling_ = false; RCLCPP_INFO( - this->get_logger(), "L:%d, ML:%d, MR:%d, R:%d", - sampling_values_[LEFT], sampling_values_[MID_LEFT], - sampling_values_[MID_RIGHT], sampling_values_[RIGHT]); + this->get_logger(), "L:%d, ML:%d, MR:%d, R:%d", sampling_values_[LEFT], + sampling_values_[MID_LEFT], sampling_values_[MID_RIGHT], sampling_values_[RIGHT]); set_line_thresholds(); beep_success(); @@ -281,14 +278,13 @@ void Follower::set_line_thresholds(void) } for (int sensor_i = 0; sensor_i < SENSOR_NUM; sensor_i++) { - line_thresholds_[sensor_i] = median( - sensor_line_values_[sensor_i], sensor_field_values_[sensor_i]); + line_thresholds_[sensor_i] = + median(sensor_line_values_[sensor_i], sensor_field_values_[sensor_i]); } RCLCPP_INFO( - this->get_logger(), "line_thresholds: L:%d, ML:%d, MR:%d, R:%d", - line_thresholds_[LEFT], line_thresholds_[MID_LEFT], - line_thresholds_[MID_RIGHT], line_thresholds_[RIGHT]); + this->get_logger(), "line_thresholds: L:%d, ML:%d, MR:%d, R:%d", line_thresholds_[LEFT], + line_thresholds_[MID_LEFT], line_thresholds_[MID_RIGHT], line_thresholds_[RIGHT]); } CallbackReturn Follower::on_configure(const rclcpp_lifecycle::State &) @@ -301,7 +297,7 @@ CallbackReturn Follower::on_configure(const rclcpp_lifecycle::State &) // Don't actually start publishing data until activated cmd_vel_timer_->cancel(); - cmd_vel_pub_ = create_publisher("cmd_vel", 1); + cmd_vel_pub_ = create_publisher("cmd_vel", 1); buzzer_pub_ = create_publisher("buzzer", 1); leds_pub_ = create_publisher("leds", 1); light_sensors_sub_ = create_subscription( @@ -311,9 +307,7 @@ CallbackReturn Follower::on_configure(const rclcpp_lifecycle::State &) 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."); + RCLCPP_ERROR(this->get_logger(), "Service motor_power is not avaliable."); return CallbackReturn::FAILURE; } return CallbackReturn::SUCCESS; diff --git a/src/object_tracking_component.cpp b/src/object_tracking_component.cpp index 0673839..d55ab0b 100644 --- a/src/object_tracking_component.cpp +++ b/src/object_tracking_component.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 RT Corporation +// Copyright 2020-2024 RT Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -25,7 +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" +#include "cv_bridge/cv_bridge.hpp" using namespace std::chrono_literals; using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -34,8 +34,7 @@ namespace object_tracking { Tracker::Tracker(const rclcpp::NodeOptions & options) -: rclcpp_lifecycle::LifecycleNode("tracker", options), - object_is_detected_(false) +: rclcpp_lifecycle::LifecycleNode("tracker", options), object_is_detected_(false) { } @@ -59,21 +58,21 @@ void Tracker::image_callback(const sensor_msgs::msg::Image::SharedPtr msg_image) 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.1; // 0.0 ~ 1.0 + const double LINEAR_VEL = -0.5; // unit: m/s + const double ANGULAR_VEL = -0.8; // unit: rad/s + const double TARGET_AREA = 0.1; // 0.0 ~ 1.0 const double OBJECT_AREA_THRESHOLD = 0.01; // 0.0 ~ 1.0 // Detects an object and tracks it // when the number of pixels of the object is greater than the threshold. if (object_is_detected_ && object_normalized_area_ > OBJECT_AREA_THRESHOLD) { - cmd_vel_.linear.x = LINEAR_VEL * (object_normalized_area_ - TARGET_AREA); - cmd_vel_.angular.z = ANGULAR_VEL * object_normalized_point_.x; + cmd_vel_.twist.linear.x = LINEAR_VEL * (object_normalized_area_ - TARGET_AREA); + cmd_vel_.twist.angular.z = ANGULAR_VEL * object_normalized_point_.x; } else { - cmd_vel_.linear.x = 0.0; - cmd_vel_.angular.z = 0.0; + cmd_vel_.twist.linear.x = 0.0; + cmd_vel_.twist.angular.z = 0.0; } - auto msg = std::make_unique(cmd_vel_); + auto msg = std::make_unique(cmd_vel_); cmd_vel_pub_->publish(std::move(msg)); } @@ -95,8 +94,7 @@ std::string Tracker::mat_type2encoding(int mat_type) } // Ref: https://github.com/ros2/demos/blob/dashing/image_tools/src/cam2image.cpp -void Tracker::convert_frame_to_message( - const cv::Mat & frame, sensor_msgs::msg::Image & msg) +void Tracker::convert_frame_to_message(const cv::Mat & frame, sensor_msgs::msg::Image & msg) { // copy cv information into ros message msg.height = frame.rows; @@ -147,21 +145,17 @@ void Tracker::tracking(const cv::Mat & input_frame, cv::Mat & result_frame) // 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 - ); + 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); object_is_detected_ = true; 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); + 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, 0, 0), 2); + result_frame, text, cv::Point(0, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 0, 0), 2); } else { object_is_detected_ = false; } @@ -176,7 +170,7 @@ CallbackReturn Tracker::on_configure(const rclcpp_lifecycle::State &) cmd_vel_timer_->cancel(); result_image_pub_ = create_publisher("result_image", 1); - cmd_vel_pub_ = create_publisher("cmd_vel", 1); + cmd_vel_pub_ = create_publisher("cmd_vel", 1); image_sub_ = create_subscription( "camera/color/image_raw", rclcpp::SensorDataQoS(), std::bind(&Tracker::image_callback, this, std::placeholders::_1)); @@ -190,9 +184,7 @@ CallbackReturn Tracker::on_activate(const rclcpp_lifecycle::State &) 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."); + RCLCPP_ERROR(this->get_logger(), "Service motor_power is not avaliable."); return CallbackReturn::FAILURE; } auto request = std::make_shared(); @@ -214,7 +206,7 @@ CallbackReturn Tracker::on_deactivate(const rclcpp_lifecycle::State &) cmd_vel_timer_->cancel(); object_is_detected_ = false; - cmd_vel_ = geometry_msgs::msg::Twist(); + cmd_vel_ = geometry_msgs::msg::TwistStamped(); return CallbackReturn::SUCCESS; }