From 304f82b30aa511b7dc329442b98466815c5e7069 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Wed, 20 Nov 2024 10:00:38 +0900 Subject: [PATCH] Revert "Adapt clang-format with Google style" This reverts commit 0a420431a45f3d96bc840e58502d364106c554f0. --- .../camera_line_follower_component.hpp | 34 ++-- .../direction_controller_component.hpp | 52 +++--- .../line_follower_component.hpp | 50 +++--- .../object_tracking_component.hpp | 34 ++-- .../visibility_control.h | 51 +++--- src/camera_line_follower_component.cpp | 137 ++++++++------- src/direction_controller.cpp | 8 +- src/direction_controller_component.cpp | 104 ++++++----- src/lifecycle_node_manager.cpp | 115 +++++++----- src/line_follower.cpp | 5 +- src/line_follower_component.cpp | 165 ++++++++++-------- src/object_tracking_component.cpp | 123 ++++++------- 12 files changed, 476 insertions(+), 402 deletions(-) diff --git a/include/raspimouse_ros2_examples/camera_line_follower_component.hpp b/include/raspimouse_ros2_examples/camera_line_follower_component.hpp index abb1d54..92b585b 100644 --- a/include/raspimouse_ros2_examples/camera_line_follower_component.hpp +++ b/include/raspimouse_ros2_examples/camera_line_follower_component.hpp @@ -18,40 +18,39 @@ #include #include -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "opencv2/highgui/highgui.hpp" #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 "sensor_msgs/msg/image.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_stamped.hpp" +#include "opencv2/highgui/highgui.hpp" -namespace camera_line_follower { +namespace camera_line_follower +{ -class CameraFollower : public rclcpp_lifecycle::LifecycleNode { - public: +class CameraFollower : public rclcpp_lifecycle::LifecycleNode +{ +public: RASPIMOUSE_ROS2_EXAMPLES_PUBLIC - explicit CameraFollower(const rclcpp::NodeOptions &options); + explicit CameraFollower(const rclcpp::NodeOptions & options); - protected: +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: +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< - rclcpp_lifecycle::LifecyclePublisher> - cmd_vel_pub_; + 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_; @@ -59,10 +58,11 @@ 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); + 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/include/raspimouse_ros2_examples/direction_controller_component.hpp b/include/raspimouse_ros2_examples/direction_controller_component.hpp index 6fbf411..3bade70 100644 --- a/include/raspimouse_ros2_examples/direction_controller_component.hpp +++ b/include/raspimouse_ros2_examples/direction_controller_component.hpp @@ -19,30 +19,31 @@ #include #include -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "geometry_msgs/msg/vector3.hpp" -#include "raspimouse_msgs/msg/switches.hpp" #include "raspimouse_ros2_examples/visibility_control.h" +#include "raspimouse_msgs/msg/switches.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/imu.hpp" -#include "std_msgs/msg/float64.hpp" #include "std_msgs/msg/int16.hpp" +#include "std_msgs/msg/float64.hpp" #include "std_msgs/msg/string.hpp" #include "std_srvs/srv/set_bool.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "geometry_msgs/msg/vector3.hpp" -namespace direction_controller { +namespace direction_controller +{ -class PIDController { - public: +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) {} - - double update(const double current, const double target) { + : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0), + error1_(0.0), error2_(0.0), output_(0.0) + { + } + + double update(const double current, const double target) + { double error = target - current; double delta_output = p_gain_ * (error - error1_); @@ -57,19 +58,21 @@ class PIDController { return output_; } - void set_gain(const double p_gain, const double i_gain, const double d_gain) { + void set_gain(const double p_gain, const double i_gain, const double d_gain) + { p_gain_ = p_gain; i_gain_ = i_gain; d_gain_ = d_gain; } - void reset_output_and_errors() { + void reset_output_and_errors() + { error1_ = 0.0; error2_ = 0.0; output_ = 0.0; } - private: +private: double p_gain_; double i_gain_; double d_gain_; @@ -78,15 +81,16 @@ class PIDController { double output_; }; -class Controller : public rclcpp::Node { - public: +class Controller : public rclcpp::Node +{ +public: RASPIMOUSE_ROS2_EXAMPLES_PUBLIC - explicit Controller(const rclcpp::NodeOptions& options); + explicit Controller(const rclcpp::NodeOptions & options); - protected: +protected: void on_cmd_vel_timer(); - private: +private: raspimouse_msgs::msg::Switches switches_; sensor_msgs::msg::Imu imu_data_raw_; @@ -120,7 +124,7 @@ class Controller : public rclcpp::Node { void calculate_heading_angle(const double omega, const double current_time); void angle_control(const double target_angle); void rotation(void); - void beep_buzzer(const int freq, const std::chrono::nanoseconds& beep_time); + void beep_buzzer(const int freq, const std::chrono::nanoseconds & beep_time); void beep_start(void); void beep_success(void); void beep_failure(void); diff --git a/include/raspimouse_ros2_examples/line_follower_component.hpp b/include/raspimouse_ros2_examples/line_follower_component.hpp index 3f7eb84..95e037f 100644 --- a/include/raspimouse_ros2_examples/line_follower_component.hpp +++ b/include/raspimouse_ros2_examples/line_follower_component.hpp @@ -19,32 +19,41 @@ #include #include -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "raspimouse_msgs/msg/leds.hpp" -#include "raspimouse_msgs/msg/light_sensors.hpp" -#include "raspimouse_msgs/msg/switches.hpp" #include "raspimouse_ros2_examples/visibility_control.h" +#include "raspimouse_msgs/msg/switches.hpp" +#include "raspimouse_msgs/msg/light_sensors.hpp" +#include "raspimouse_msgs/msg/leds.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "std_msgs/msg/int16.hpp" #include "std_msgs/msg/string.hpp" #include "std_srvs/srv/set_bool.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" -namespace line_follower { +namespace line_follower +{ -class Follower : public rclcpp_lifecycle::LifecycleNode { - public: +class Follower : public rclcpp_lifecycle::LifecycleNode +{ +public: RASPIMOUSE_ROS2_EXAMPLES_PUBLIC - explicit Follower(const rclcpp::NodeOptions &options); + explicit Follower(const rclcpp::NodeOptions & options); - protected: +protected: void on_cmd_vel_timer(); - private: +private: using SensorsType = std::vector; - enum SensorIndex { LEFT = 0, MID_LEFT, MID_RIGHT, RIGHT, SENSOR_NUM }; + enum SensorIndex + { + LEFT = 0, + MID_LEFT, + MID_RIGHT, + RIGHT, + SENSOR_NUM + }; static const int NUM_OF_SAMPLES; @@ -62,23 +71,16 @@ class Follower : public rclcpp_lifecycle::LifecycleNode { bool field_sampling_; bool can_publish_cmdvel_; - std::shared_ptr> - buzzer_pub_; - std::shared_ptr< - rclcpp_lifecycle::LifecyclePublisher> - cmd_vel_pub_; - std::shared_ptr< - rclcpp_lifecycle::LifecyclePublisher> - leds_pub_; - rclcpp::Subscription::SharedPtr - light_sensors_sub_; + std::shared_ptr> buzzer_pub_; + std::shared_ptr> cmd_vel_pub_; + std::shared_ptr> leds_pub_; + rclcpp::Subscription::SharedPtr light_sensors_sub_; rclcpp::Subscription::SharedPtr switches_sub_; std::shared_ptr> motor_power_client_; rclcpp::TimerBase::SharedPtr cmd_vel_timer_; - void callback_light_sensors( - const raspimouse_msgs::msg::LightSensors::SharedPtr msg); + void callback_light_sensors(const raspimouse_msgs::msg::LightSensors::SharedPtr msg); void callback_switches(const raspimouse_msgs::msg::Switches::SharedPtr msg); void set_motor_power(const bool motor_on); @@ -86,7 +88,7 @@ class Follower : public rclcpp_lifecycle::LifecycleNode { void update_line_detection(void); bool line_is_bright(void); void indicate_line_detections(void); - void beep_buzzer(const int freq, const std::chrono::nanoseconds &beep_time); + void beep_buzzer(const int freq, const std::chrono::nanoseconds & beep_time); void beep_start(void); void beep_success(void); void beep_failure(void); diff --git a/include/raspimouse_ros2_examples/object_tracking_component.hpp b/include/raspimouse_ros2_examples/object_tracking_component.hpp index 063d707..8dd9cbb 100644 --- a/include/raspimouse_ros2_examples/object_tracking_component.hpp +++ b/include/raspimouse_ros2_examples/object_tracking_component.hpp @@ -18,47 +18,47 @@ #include #include -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "opencv2/highgui/highgui.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 "sensor_msgs/msg/image.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_stamped.hpp" +#include "opencv2/highgui/highgui.hpp" -namespace object_tracking { +namespace object_tracking +{ -class Tracker : public rclcpp_lifecycle::LifecycleNode { - public: +class Tracker : public rclcpp_lifecycle::LifecycleNode +{ +public: RASPIMOUSE_ROS2_EXAMPLES_PUBLIC - explicit Tracker(const rclcpp::NodeOptions &options); + explicit Tracker(const rclcpp::NodeOptions & options); - protected: +protected: void image_callback(const sensor_msgs::msg::Image::SharedPtr msg_image); void on_cmd_vel_timer(); - private: +private: cv::VideoCapture cap_; bool object_is_detected_; cv::Point2d object_normalized_point_; double object_normalized_area_; geometry_msgs::msg::TwistStamped cmd_vel_; - std::shared_ptr> - result_image_pub_; - std::shared_ptr< - rclcpp_lifecycle::LifecyclePublisher> - cmd_vel_pub_; + std::shared_ptr> result_image_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); + void tracking(const cv::Mat & input_frame, cv::Mat & result_frame); rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State &); diff --git a/include/raspimouse_ros2_examples/visibility_control.h b/include/raspimouse_ros2_examples/visibility_control.h index b9fbf35..40d4c42 100644 --- a/include/raspimouse_ros2_examples/visibility_control.h +++ b/include/raspimouse_ros2_examples/visibility_control.h @@ -16,38 +16,39 @@ #define RASPIMOUSE_ROS2_EXAMPLES__VISIBILITY_CONTROL_H_ #ifdef __cplusplus -extern "C" { +extern "C" +{ #endif // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define RASPIMOUSE_ROS2_EXAMPLES_EXPORT __attribute__((dllexport)) -#define RASPIMOUSE_ROS2_EXAMPLES_IMPORT __attribute__((dllimport)) + #ifdef __GNUC__ + #define RASPIMOUSE_ROS2_EXAMPLES_EXPORT __attribute__ ((dllexport)) + #define RASPIMOUSE_ROS2_EXAMPLES_IMPORT __attribute__ ((dllimport)) + #else + #define RASPIMOUSE_ROS2_EXAMPLES_EXPORT __declspec(dllexport) + #define RASPIMOUSE_ROS2_EXAMPLES_IMPORT __declspec(dllimport) + #endif + #ifdef RASPIMOUSE_ROS2_EXAMPLES_BUILDING_DLL + #define RASPIMOUSE_ROS2_EXAMPLES_PUBLIC RASPIMOUSE_ROS2_EXAMPLES_EXPORT + #else + #define RASPIMOUSE_ROS2_EXAMPLES_PUBLIC RASPIMOUSE_ROS2_EXAMPLES_IMPORT + #endif + #define RASPIMOUSE_ROS2_EXAMPLES_PUBLIC_TYPE RASPIMOUSE_ROS2_EXAMPLES_PUBLIC + #define RASPIMOUSE_ROS2_EXAMPLES_LOCAL #else -#define RASPIMOUSE_ROS2_EXAMPLES_EXPORT __declspec(dllexport) -#define RASPIMOUSE_ROS2_EXAMPLES_IMPORT __declspec(dllimport) -#endif -#ifdef RASPIMOUSE_ROS2_EXAMPLES_BUILDING_DLL -#define RASPIMOUSE_ROS2_EXAMPLES_PUBLIC RASPIMOUSE_ROS2_EXAMPLES_EXPORT -#else -#define RASPIMOUSE_ROS2_EXAMPLES_PUBLIC RASPIMOUSE_ROS2_EXAMPLES_IMPORT -#endif -#define RASPIMOUSE_ROS2_EXAMPLES_PUBLIC_TYPE RASPIMOUSE_ROS2_EXAMPLES_PUBLIC -#define RASPIMOUSE_ROS2_EXAMPLES_LOCAL -#else -#define RASPIMOUSE_ROS2_EXAMPLES_EXPORT __attribute__((visibility("default"))) -#define RASPIMOUSE_ROS2_EXAMPLES_IMPORT -#if __GNUC__ >= 4 -#define RASPIMOUSE_ROS2_EXAMPLES_PUBLIC __attribute__((visibility("default"))) -#define RASPIMOUSE_ROS2_EXAMPLES_LOCAL __attribute__((visibility("hidden"))) -#else -#define RASPIMOUSE_ROS2_EXAMPLES_PUBLIC -#define RASPIMOUSE_ROS2_EXAMPLES_LOCAL -#endif -#define RASPIMOUSE_ROS2_EXAMPLES_PUBLIC_TYPE + #define RASPIMOUSE_ROS2_EXAMPLES_EXPORT __attribute__ ((visibility("default"))) + #define RASPIMOUSE_ROS2_EXAMPLES_IMPORT + #if __GNUC__ >= 4 + #define RASPIMOUSE_ROS2_EXAMPLES_PUBLIC __attribute__ ((visibility("default"))) + #define RASPIMOUSE_ROS2_EXAMPLES_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RASPIMOUSE_ROS2_EXAMPLES_PUBLIC + #define RASPIMOUSE_ROS2_EXAMPLES_LOCAL + #endif + #define RASPIMOUSE_ROS2_EXAMPLES_PUBLIC_TYPE #endif #ifdef __cplusplus diff --git a/src/camera_line_follower_component.cpp b/src/camera_line_follower_component.cpp index 0b7068e..9261f34 100644 --- a/src/camera_line_follower_component.cpp +++ b/src/camera_line_follower_component.cpp @@ -14,19 +14,19 @@ #include "raspimouse_ros2_examples/camera_line_follower_component.hpp" +#include #include #include -#include -#include -#include #include +#include #include -#include "cv_bridge/cv_bridge.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "lifecycle_msgs/srv/change_state.hpp" +#include #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" +#include "cv_bridge/cv_bridge.hpp" constexpr auto MIN_BRIGHTNESS_PARAM = "min_brightness"; constexpr auto MAX_BRIGHTNESS_PARAM = "max_brightness"; @@ -34,17 +34,20 @@ 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 { + +namespace camera_line_follower +{ using namespace std::chrono_literals; -using CallbackReturn = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -CameraFollower::CameraFollower(const rclcpp::NodeOptions &options) - : rclcpp_lifecycle::LifecycleNode("camera_follower", options), - object_is_detected_(false) {} +CameraFollower::CameraFollower(const rclcpp::NodeOptions & options) +: rclcpp_lifecycle::LifecycleNode("camera_follower", options), + object_is_detected_(false) +{ +} -void CameraFollower::image_callback( - const sensor_msgs::msg::Image::SharedPtr msg_image) { +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(); @@ -58,8 +61,8 @@ void CameraFollower::image_callback( } } -void CameraFollower::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."); set_motor_power(false); @@ -71,17 +74,17 @@ void CameraFollower::callback_switches( } } -void CameraFollower::on_cmd_vel_timer() { +void CameraFollower::on_cmd_vel_timer() +{ 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_ && - object_normalized_area_ > - get_parameter(AREA_THRESHOLD_PARAM).as_double()) { + object_normalized_area_ > get_parameter(AREA_THRESHOLD_PARAM).as_double()) + { 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; + cmd_vel.twist.angular.z = -get_parameter(ANGULAR_VEL_PARAM).as_double() * object_normalized_point_.x; } else { cmd_vel.twist.linear.x = 0.0; cmd_vel.twist.angular.z = 0.0; @@ -96,9 +99,12 @@ void CameraFollower::on_cmd_vel_timer() { cmd_vel_pub_->publish(std::move(msg)); } -void CameraFollower::set_motor_power(const bool motor_on) { +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(); @@ -107,7 +113,8 @@ void CameraFollower::set_motor_power(const bool motor_on) { } // Ref: https://github.com/ros2/demos/blob/dashing/image_tools/src/cam2image.cpp -std::string CameraFollower::mat_type2encoding(const int mat_type) const { +std::string CameraFollower::mat_type2encoding(const int mat_type) const +{ switch (mat_type) { case CV_8UC1: return "mono8"; @@ -124,7 +131,8 @@ std::string CameraFollower::mat_type2encoding(const int mat_type) const { // Ref: https://github.com/ros2/demos/blob/dashing/image_tools/src/cam2image.cpp void CameraFollower::convert_frame_to_message( - const cv::Mat &frame, sensor_msgs::msg::Image &msg) const { + const cv::Mat & frame, sensor_msgs::msg::Image & msg) const +{ // copy cv information into ros message msg.height = frame.rows; msg.width = frame.cols; @@ -136,15 +144,17 @@ void CameraFollower::convert_frame_to_message( msg.header.frame_id = "camera_frame"; } -bool CameraFollower::detect_line(const cv::Mat &input_frame, - cv::Mat &result_frame) { - // Specific colors are extracted from the input image and converted to binary - // values. +bool CameraFollower::detect_line(const cv::Mat & input_frame, cv::Mat & result_frame) +{ + // Specific colors are extracted from the input image and converted to binary values. cv::Mat gray; cv::cvtColor(input_frame, gray, cv::COLOR_BGR2GRAY); cv::Mat extracted_bin; - cv::inRange(gray, get_parameter(MIN_BRIGHTNESS_PARAM).as_int(), - get_parameter(MAX_BRIGHTNESS_PARAM).as_int(), extracted_bin); + cv::inRange( + gray, + get_parameter(MIN_BRIGHTNESS_PARAM).as_int(), + get_parameter(MAX_BRIGHTNESS_PARAM).as_int(), + extracted_bin); input_frame.copyTo(result_frame, extracted_bin); // Remove noise with morphology transformation @@ -154,8 +164,7 @@ bool CameraFollower::detect_line(const cv::Mat &input_frame, // Extracting contours std::vector> contours; std::vector hierarchy; - cv::findContours(morph_bin, contours, hierarchy, cv::RETR_EXTERNAL, - cv::CHAIN_APPROX_SIMPLE); + cv::findContours(morph_bin, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); // Extracting the largest contours double max_area = 0; @@ -168,58 +177,56 @@ bool CameraFollower::detect_line(const cv::Mat &input_frame, } } - // If the contour exists (if the object exists), find the centroid of the - // contour + // 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); + 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); + std::string text = "Area:" + std::to_string(object_normalized_area_ * 100) + "%"; + cv::drawContours( + result_frame, contours, max_area_index, + cv::Scalar(0, 255, 0), 2, cv::LINE_4, hierarchy); + cv::circle(result_frame, mt_point, 30, cv::Scalar(0, 0, 255), 2, cv::LINE_4); + cv::putText( + result_frame, text, cv::Point(0, 30), + cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255), 2); return true; } else { return false; } } -CallbackReturn CameraFollower::on_configure(const rclcpp_lifecycle::State &) { +CallbackReturn CameraFollower::on_configure(const rclcpp_lifecycle::State &) +{ RCLCPP_INFO(this->get_logger(), "on_configure() is called."); - cmd_vel_timer_ = create_wall_timer( - 50ms, std::bind(&CameraFollower::on_cmd_vel_timer, this)); + cmd_vel_timer_ = create_wall_timer(50ms, std::bind(&CameraFollower::on_cmd_vel_timer, this)); // Don't actually start publishing data until activated cmd_vel_timer_->cancel(); motor_power_client_ = create_client("motor_power"); if (!motor_power_client_->wait_for_service(5s)) { - RCLCPP_ERROR(this->get_logger(), "Service motor_power is not avaliable."); + 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); + result_image_pub_ = create_publisher("result_image", 1); + cmd_vel_pub_ = create_publisher("cmd_vel", 1); image_sub_ = create_subscription( - "camera/color/image_raw", rclcpp::SensorDataQoS(), - std::bind(&CameraFollower::image_callback, this, std::placeholders::_1)); + "camera/color/image_raw", rclcpp::SensorDataQoS(), + std::bind(&CameraFollower::image_callback, this, std::placeholders::_1)); switches_sub_ = create_subscription( - "switches", 1, - std::bind(&CameraFollower::callback_switches, this, - std::placeholders::_1)); + "switches", 1, std::bind(&CameraFollower::callback_switches, this, std::placeholders::_1)); // Set parameter defaults declare_parameter(MIN_BRIGHTNESS_PARAM, 0); @@ -231,7 +238,8 @@ CallbackReturn CameraFollower::on_configure(const rclcpp_lifecycle::State &) { return CallbackReturn::SUCCESS; } -CallbackReturn CameraFollower::on_activate(const rclcpp_lifecycle::State &) { +CallbackReturn CameraFollower::on_activate(const rclcpp_lifecycle::State &) +{ RCLCPP_INFO(this->get_logger(), "on_activate() is called."); result_image_pub_->on_activate(); @@ -241,7 +249,8 @@ CallbackReturn CameraFollower::on_activate(const rclcpp_lifecycle::State &) { return CallbackReturn::SUCCESS; } -CallbackReturn CameraFollower::on_deactivate(const rclcpp_lifecycle::State &) { +CallbackReturn CameraFollower::on_deactivate(const rclcpp_lifecycle::State &) +{ RCLCPP_INFO(this->get_logger(), "on_deactivate() is called."); set_motor_power(false); @@ -255,7 +264,8 @@ CallbackReturn CameraFollower::on_deactivate(const rclcpp_lifecycle::State &) { return CallbackReturn::SUCCESS; } -CallbackReturn CameraFollower::on_cleanup(const rclcpp_lifecycle::State &) { +CallbackReturn CameraFollower::on_cleanup(const rclcpp_lifecycle::State &) +{ RCLCPP_INFO(this->get_logger(), "on_cleanup() is called."); set_motor_power(false); @@ -269,7 +279,8 @@ CallbackReturn CameraFollower::on_cleanup(const rclcpp_lifecycle::State &) { return CallbackReturn::SUCCESS; } -CallbackReturn CameraFollower::on_shutdown(const rclcpp_lifecycle::State &) { +CallbackReturn CameraFollower::on_shutdown(const rclcpp_lifecycle::State &) +{ RCLCPP_INFO(this->get_logger(), "on_shutdown() is called."); set_motor_power(false); diff --git a/src/direction_controller.cpp b/src/direction_controller.cpp index 124d18a..4806cf6 100644 --- a/src/direction_controller.cpp +++ b/src/direction_controller.cpp @@ -13,18 +13,18 @@ // limitations under the License. #include +#include "rclcpp/rclcpp.hpp" #include "raspimouse/raspimouse_component.hpp" #include "raspimouse_ros2_examples/direction_controller_component.hpp" -#include "rclcpp/rclcpp.hpp" #include "rt_usb_9axisimu_driver/rt_usb_9axisimu_driver_component.hpp" -int main(int argc, char* argv[]) { +int main(int argc, char * argv[]) +{ rclcpp::init(argc, argv); rclcpp::executors::SingleThreadedExecutor exec; rclcpp::NodeOptions options; - auto direction_controller = - std::make_shared(options); + auto direction_controller = std::make_shared(options); exec.add_node(direction_controller->get_node_base_interface()); exec.spin(); rclcpp::shutdown(); diff --git a/src/direction_controller_component.cpp b/src/direction_controller_component.cpp index 000842a..ea28e7c 100644 --- a/src/direction_controller_component.cpp +++ b/src/direction_controller_component.cpp @@ -24,32 +24,31 @@ using namespace std::chrono_literals; -namespace direction_controller { +namespace direction_controller +{ -enum CONTROL_MODE { +enum CONTROL_MODE +{ MODE_NONE = 0, MODE_CALIBRATION = 1, MODE_KEEP_ZERO_RADIAN = 2, MODE_ROTATION = 3 }; -Controller::Controller(const rclcpp::NodeOptions& options) - : Node("direction_controller", options) { +Controller::Controller(const rclcpp::NodeOptions & options) +: Node("direction_controller", options) +{ using namespace std::placeholders; // for _1, _2, _3... - cmd_vel_timer_ = - create_wall_timer(16ms, std::bind(&Controller::on_cmd_vel_timer, this)); + 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); + heading_angle_pub_ = create_publisher("heading_angle", 1); switches_sub_ = create_subscription( - "switches", 1, std::bind(&Controller::callback_switches, this, _1)); + "switches", 1, std::bind(&Controller::callback_switches, this, _1)); imu_data_raw_sub_ = create_subscription( - "imu/data_raw", 1, - std::bind(&Controller::callback_imu_data_raw, this, _1)); + "imu/data_raw", 1, std::bind(&Controller::callback_imu_data_raw, this, _1)); motor_power_client_ = create_client("motor_power"); @@ -73,9 +72,10 @@ Controller::Controller(const rclcpp::NodeOptions& options) descriptor.floating_point_range[0] = range; 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("d_gain").as_double()); + omega_pid_controller_.set_gain( + 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; control_mode_ = MODE_NONE; @@ -86,7 +86,8 @@ Controller::Controller(const rclcpp::NodeOptions& options) increase_target_angle_ = true; } -void Controller::on_cmd_vel_timer() { +void Controller::on_cmd_vel_timer() +{ int released_switch_number = -1; if (switches_.switch0) { pressed_switch_number_ = 0; @@ -102,13 +103,13 @@ 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("d_gain").as_double()); + omega_pid_controller_.set_gain( + 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) { - if (control_mode_ == MODE_KEEP_ZERO_RADIAN || - control_mode_ == MODE_ROTATION) { + if (control_mode_ == MODE_KEEP_ZERO_RADIAN || control_mode_ == MODE_ROTATION) { control_mode_ = MODE_NONE; set_motor_power(false); return; @@ -139,17 +140,16 @@ void Controller::on_cmd_vel_timer() { } } -void Controller::callback_switches( - const raspimouse_msgs::msg::Switches::SharedPtr msg) { +void Controller::callback_switches(const raspimouse_msgs::msg::Switches::SharedPtr msg) +{ switches_ = *msg; } -void Controller::callback_imu_data_raw( - const sensor_msgs::msg::Imu::SharedPtr msg) { +void Controller::callback_imu_data_raw(const sensor_msgs::msg::Imu::SharedPtr msg) +{ imu_data_raw_ = *msg; - calculate_heading_angle(imu_data_raw_.angular_velocity.z, - this->now().seconds()); + calculate_heading_angle(imu_data_raw_.angular_velocity.z, this->now().seconds()); auto heading_angle_msg = std::make_unique(); heading_angle_msg->data = heading_angle_; heading_angle_pub_->publish(std::move(heading_angle_msg)); @@ -167,9 +167,12 @@ void Controller::callback_imu_data_raw( filter_acceleration(imu_data_raw_.linear_acceleration); } -bool Controller::set_motor_power(const bool motor_on) { +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(); @@ -178,16 +181,17 @@ bool Controller::set_motor_power(const bool motor_on) { return true; } -bool Controller::omega_calibration(const double omega) { +bool Controller::omega_calibration(const double omega) +{ const int SAMPLE_NUM = 100; bool complete = false; 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; } @@ -195,8 +199,8 @@ bool Controller::omega_calibration(const double omega) { return complete; } -void Controller::calculate_heading_angle(const double omega, - const double current_time) { +void Controller::calculate_heading_angle(const double omega, const double current_time) +{ const double ALPHA = 1.0; double diff_time = current_time - prev_heading_calculation_time_; @@ -206,7 +210,8 @@ void Controller::calculate_heading_angle(const double omega, prev_heading_calculation_time_ = current_time; } -void Controller::filter_acceleration(const geometry_msgs::msg::Vector3 acc) { +void Controller::filter_acceleration(const geometry_msgs::msg::Vector3 acc) +{ const double ALPHA = 0.1; // Simple low pass filter @@ -216,17 +221,18 @@ void Controller::filter_acceleration(const geometry_msgs::msg::Vector3 acc) { prev_acc_ = filtered_acc_; } -void Controller::angle_control(const double target_angle) { +void Controller::angle_control(const double target_angle) +{ const double SIGN = 1.0; auto cmd_vel = std::make_unique(); - cmd_vel->twist.angular.z = - SIGN * omega_pid_controller_.update(heading_angle_, target_angle); + cmd_vel->twist.angular.z = SIGN * omega_pid_controller_.update(heading_angle_, target_angle); cmd_vel_pub_->publish(std::move(cmd_vel)); } -void Controller::rotation(void) { +void Controller::rotation(void) +{ const double ADD_ANGLE = 2.0 * M_PI / 180.0; const double START_ANGLE = -M_PI * 0.5; const double END_ANGLE = M_PI * 0.5; @@ -248,8 +254,8 @@ void Controller::rotation(void) { angle_control(target_angle_); } -void Controller::beep_buzzer(const int freq, - const std::chrono::nanoseconds& beep_time) { +void Controller::beep_buzzer(const int freq, const std::chrono::nanoseconds & beep_time) +{ auto msg = std::make_unique(); msg->data = freq; buzzer_pub_->publish(std::move(msg)); @@ -261,21 +267,27 @@ void Controller::beep_buzzer(const int freq, 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) { +void Controller::beep_success(void) +{ beep_buzzer(1000, 100ms); rclcpp::sleep_for(100ms); beep_buzzer(1000, 100ms); } -void Controller::beep_failure(void) { +void Controller::beep_failure(void) +{ for (int i = 0; i < 4; i++) { beep_buzzer(500, 100ms); rclcpp::sleep_for(100ms); } } + } // 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 59d26df..4907037 100644 --- a/src/lifecycle_node_manager.cpp +++ b/src/lifecycle_node_manager.cpp @@ -22,35 +22,38 @@ #include "lifecycle_msgs/msg/transition.hpp" #include "lifecycle_msgs/srv/change_state.hpp" #include "lifecycle_msgs/srv/get_state.hpp" + #include "rclcpp/rclcpp.hpp" + using namespace std::chrono_literals; using MsgState = lifecycle_msgs::msg::State; using MsgTransition = lifecycle_msgs::msg::Transition; 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::uint8_t state_of( + 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; } auto future_result = client->async_send_request(request); - auto future_status = - rclcpp::spin_until_future_complete(node, future_result, time_out); + auto future_status = rclcpp::spin_until_future_complete(node, future_result, time_out); if (future_status != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR(node->get_logger(), - "Service %s time out while getting current state.", - service_name.c_str()); + RCLCPP_ERROR( + node->get_logger(), + "Service %s time out while getting current state.", service_name.c_str()); return MsgState::PRIMARY_STATE_UNKNOWN; } @@ -58,33 +61,42 @@ std::uint8_t state_of(std::string target_node_name, } 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; - }); + 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) { +bool all_nodes_are_inactive( + 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; - }); + 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) { +bool all_nodes_are_active( + 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; - }); + 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) { +bool change_state( + 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; @@ -92,42 +104,50 @@ bool change_state(std::string target_node_name, rclcpp::Node::SharedPtr node, 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; } auto future_result = client->async_send_request(request); - auto future_status = - rclcpp::spin_until_future_complete(node, future_result, time_out); + auto future_status = rclcpp::spin_until_future_complete(node, future_result, time_out); if (future_status != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR(node->get_logger(), - "Service %s time out while changing current state.", - service_name.c_str()); + RCLCPP_ERROR( + node->get_logger(), + "Service %s time out while changing current state.", service_name.c_str()); return false; } return future_result.get()->success; } -bool configure_all_nodes(rclcpp::Node::SharedPtr node, - const std::vector& target_node_names) { +bool configure_all_nodes( + 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); - }); + 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) { +bool activate_all_nodes( + 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); - }); + 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[]) { + +int main(int argc, char * argv[]) +{ // Force flush of the stdout buffer. setvbuf(stdout, NULL, _IONBF, BUFSIZ); @@ -136,8 +156,7 @@ int main(int argc, char* argv[]) { auto node = rclcpp::Node::make_shared("lifecycle_node_manager"); node->declare_parameter("components", std::vector()); - auto components = - node->get_parameter("components").get_value>(); + auto components = node->get_parameter("components").get_value>(); if (components.size() == 0) { RCLCPP_ERROR(node->get_logger(), "param 'components' has no value."); diff --git a/src/line_follower.cpp b/src/line_follower.cpp index 3d0bb09..1392631 100644 --- a/src/line_follower.cpp +++ b/src/line_follower.cpp @@ -13,12 +13,13 @@ // limitations under the License. #include +#include "rclcpp/rclcpp.hpp" #include "raspimouse/raspimouse_component.hpp" #include "raspimouse_ros2_examples/line_follower_component.hpp" -#include "rclcpp/rclcpp.hpp" -int main(int argc, char* argv[]) { +int main(int argc, char * argv[]) +{ rclcpp::init(argc, argv); rclcpp::executors::MultiThreadedExecutor exec; rclcpp::NodeOptions options; diff --git a/src/line_follower_component.cpp b/src/line_follower_component.cpp index 0a0aaf1..18be314 100644 --- a/src/line_follower_component.cpp +++ b/src/line_follower_component.cpp @@ -15,42 +15,43 @@ #include "raspimouse_ros2_examples/line_follower_component.hpp" #include +#include #include #include #include -#include -#include #include +#include #include -#include "lifecycle_msgs/srv/change_state.hpp" #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; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -namespace line_follower { +namespace line_follower +{ const int Follower::NUM_OF_SAMPLES = 10; -Follower::Follower(const rclcpp::NodeOptions &options) - : rclcpp_lifecycle::LifecycleNode("follower", options), - present_sensor_values_(SensorsType(SENSOR_NUM, 0)), - sensor_line_values_(SensorsType(SENSOR_NUM, 0)), - sensor_field_values_(SensorsType(SENSOR_NUM, 0)), - line_thresholds_(SensorsType(SENSOR_NUM, 0)), - 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), - can_publish_cmdvel_(false) {} - -void Follower::on_cmd_vel_timer() { +Follower::Follower(const rclcpp::NodeOptions & options) +: rclcpp_lifecycle::LifecycleNode("follower", options), + present_sensor_values_(SensorsType(SENSOR_NUM, 0)), + sensor_line_values_(SensorsType(SENSOR_NUM, 0)), + sensor_field_values_(SensorsType(SENSOR_NUM, 0)), + line_thresholds_(SensorsType(SENSOR_NUM, 0)), + 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), + can_publish_cmdvel_(false) +{ +} + +void Follower::on_cmd_vel_timer() +{ if (line_sampling_ || field_sampling_) { return; } @@ -86,10 +87,9 @@ void Follower::on_cmd_vel_timer() { indicate_line_detections(); } -void Follower::callback_light_sensors( - const raspimouse_msgs::msg::LightSensors::SharedPtr msg) { - // The order of the front distance sensors and the line following sensors are - // not same +void Follower::callback_light_sensors(const raspimouse_msgs::msg::LightSensors::SharedPtr msg) +{ + // The order of the front distance sensors and the line following sensors are not same present_sensor_values_[LEFT] = msg->forward_r; present_sensor_values_[MID_LEFT] = msg->right; present_sensor_values_[MID_RIGHT] = msg->left; @@ -104,30 +104,32 @@ void Follower::callback_light_sensors( } } -void Follower::callback_switches( - const raspimouse_msgs::msg::Switches::SharedPtr msg) { +void Follower::callback_switches(const raspimouse_msgs::msg::Switches::SharedPtr msg) +{ switches_ = *msg; } -void Follower::set_motor_power(const bool motor_on) { +void Follower::set_motor_power(const bool motor_on) +{ auto request = std::make_shared(); request->data = motor_on; auto future_result = motor_power_client_->async_send_request(request); } -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 +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 LOW_VEL_ANGULAR_Z = 0.5; // rad/s auto cmd_vel = std::make_unique(); - bool detect_line = std::any_of(line_is_detected_by_sensor_.begin(), - line_is_detected_by_sensor_.end(), - [](bool detected) { return detected; }); - bool detect_field = std::any_of(line_is_detected_by_sensor_.begin(), - line_is_detected_by_sensor_.end(), - [](bool detected) { return !detected; }); + bool detect_line = std::any_of( + line_is_detected_by_sensor_.begin(), line_is_detected_by_sensor_.end(), + [](bool detected) {return detected;}); + bool detect_field = std::any_of( + line_is_detected_by_sensor_.begin(), line_is_detected_by_sensor_.end(), + [](bool detected) {return !detected;}); if (detect_line && detect_field) { cmd_vel->twist.linear.x = VEL_LINEAR_X; @@ -152,10 +154,10 @@ void Follower::publish_cmdvel_for_line_following(void) { cmd_vel_pub_->publish(std::move(cmd_vel)); } -void Follower::update_line_detection(void) { +void Follower::update_line_detection(void) +{ for (int sensor_i = 0; sensor_i < SENSOR_NUM; sensor_i++) { - bool is_positive = - present_sensor_values_[sensor_i] > line_thresholds_[sensor_i]; + bool is_positive = present_sensor_values_[sensor_i] > line_thresholds_[sensor_i]; if (line_is_bright() == is_positive) { line_is_detected_by_sensor_[sensor_i] = true; @@ -165,7 +167,8 @@ void Follower::update_line_detection(void) { } } -bool Follower::line_is_bright(void) { +bool Follower::line_is_bright(void) +{ const SensorIndex SAMPLE = RIGHT; if (sensor_line_values_[SAMPLE] > sensor_field_values_[SAMPLE]) { return true; @@ -174,7 +177,8 @@ bool Follower::line_is_bright(void) { } } -void Follower::indicate_line_detections(void) { +void Follower::indicate_line_detections(void) +{ auto msg = std::make_unique(); msg->led0 = line_is_detected_by_sensor_[RIGHT]; msg->led1 = line_is_detected_by_sensor_[MID_RIGHT]; @@ -183,8 +187,8 @@ void Follower::indicate_line_detections(void) { leds_pub_->publish(std::move(msg)); } -void Follower::beep_buzzer(const int freq, - const std::chrono::nanoseconds &beep_time) { +void Follower::beep_buzzer(const int freq, const std::chrono::nanoseconds & beep_time) +{ auto msg = std::make_unique(); msg->data = freq; buzzer_pub_->publish(std::move(msg)); @@ -196,22 +200,28 @@ void Follower::beep_buzzer(const int freq, 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) { +void Follower::beep_success(void) +{ beep_buzzer(1000, 100ms); rclcpp::sleep_for(100ms); beep_buzzer(1000, 100ms); } -void Follower::beep_failure(void) { +void Follower::beep_failure(void) +{ for (int i = 0; i < 4; i++) { beep_buzzer(500, 100ms); rclcpp::sleep_for(100ms); } } -bool Follower::sampling_is_done(void) { +bool Follower::sampling_is_done(void) +{ if (line_values_are_sampled_ && field_values_are_sampled_) { return true; } else { @@ -219,7 +229,8 @@ bool Follower::sampling_is_done(void) { } } -void Follower::multisampling(void) { +void Follower::multisampling(void) +{ if (sampling_count_ < NUM_OF_SAMPLES) { for (int sensor_i = 0; sensor_i < SENSOR_NUM; sensor_i++) { sampling_values_[sensor_i] += present_sensor_values_[sensor_i]; @@ -242,16 +253,18 @@ void Follower::multisampling(void) { sampling_values_ = SensorsType(SENSOR_NUM, 0); 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]); + 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]); set_line_thresholds(); beep_success(); } } -int Follower::median(const int value1, const int value2) { +int Follower::median(const int value1, const int value2) +{ int diff = std::abs(value1 - value2); if (value1 < value2) { @@ -261,50 +274,53 @@ int Follower::median(const int value1, const int value2) { } } -void Follower::set_line_thresholds(void) { +void Follower::set_line_thresholds(void) +{ if (sampling_is_done() == false) { return; } 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]); + 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]); } -CallbackReturn Follower::on_configure(const rclcpp_lifecycle::State &) { +CallbackReturn Follower::on_configure(const rclcpp_lifecycle::State &) +{ using namespace std::placeholders; // for _1, _2, _3... RCLCPP_INFO(this->get_logger(), "on_configure() is called."); - cmd_vel_timer_ = - create_wall_timer(50ms, std::bind(&Follower::on_cmd_vel_timer, this)); + cmd_vel_timer_ = create_wall_timer(50ms, std::bind(&Follower::on_cmd_vel_timer, this)); // 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( - "light_sensors", 1, - std::bind(&Follower::callback_light_sensors, this, _1)); + "light_sensors", 1, std::bind(&Follower::callback_light_sensors, this, _1)); switches_sub_ = create_subscription( - "switches", 1, std::bind(&Follower::callback_switches, this, _1)); + "switches", 1, std::bind(&Follower::callback_switches, this, _1)); 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; } -CallbackReturn Follower::on_activate(const rclcpp_lifecycle::State &) { +CallbackReturn Follower::on_activate(const rclcpp_lifecycle::State &) +{ RCLCPP_INFO(this->get_logger(), "on_activate() is called."); buzzer_pub_->on_activate(); @@ -315,7 +331,8 @@ CallbackReturn Follower::on_activate(const rclcpp_lifecycle::State &) { return CallbackReturn::SUCCESS; } -CallbackReturn Follower::on_deactivate(const rclcpp_lifecycle::State &) { +CallbackReturn Follower::on_deactivate(const rclcpp_lifecycle::State &) +{ RCLCPP_INFO(this->get_logger(), "on_deactivate() is called."); buzzer_pub_->on_deactivate(); @@ -326,7 +343,8 @@ CallbackReturn Follower::on_deactivate(const rclcpp_lifecycle::State &) { return CallbackReturn::SUCCESS; } -CallbackReturn Follower::on_cleanup(const rclcpp_lifecycle::State &) { +CallbackReturn Follower::on_cleanup(const rclcpp_lifecycle::State &) +{ RCLCPP_INFO(this->get_logger(), "on_cleanup() is called."); buzzer_pub_.reset(); @@ -339,7 +357,8 @@ CallbackReturn Follower::on_cleanup(const rclcpp_lifecycle::State &) { return CallbackReturn::SUCCESS; } -CallbackReturn Follower::on_shutdown(const rclcpp_lifecycle::State &) { +CallbackReturn Follower::on_shutdown(const rclcpp_lifecycle::State &) +{ RCLCPP_INFO(this->get_logger(), "on_shutdown() is called."); buzzer_pub_.reset(); diff --git a/src/object_tracking_component.cpp b/src/object_tracking_component.cpp index 93abbff..367b5bc 100644 --- a/src/object_tracking_component.cpp +++ b/src/object_tracking_component.cpp @@ -14,31 +14,33 @@ #include "raspimouse_ros2_examples/object_tracking_component.hpp" +#include #include #include -#include -#include -#include #include +#include #include -#include "cv_bridge/cv_bridge.hpp" -#include "lifecycle_msgs/srv/change_state.hpp" +#include #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" +#include "cv_bridge/cv_bridge.hpp" using namespace std::chrono_literals; -using CallbackReturn = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -namespace object_tracking { +namespace object_tracking +{ -Tracker::Tracker(const rclcpp::NodeOptions &options) - : rclcpp_lifecycle::LifecycleNode("tracker", options), - object_is_detected_(false) {} +Tracker::Tracker(const rclcpp::NodeOptions & options) +: rclcpp_lifecycle::LifecycleNode("tracker", options), + object_is_detected_(false) +{ +} -void Tracker::image_callback( - const sensor_msgs::msg::Image::SharedPtr msg_image) { +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(); @@ -55,17 +57,17 @@ void Tracker::image_callback( } } -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 +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 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_.twist.linear.x = - LINEAR_VEL * (object_normalized_area_ - TARGET_AREA); + 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_.twist.linear.x = 0.0; @@ -76,7 +78,8 @@ void Tracker::on_cmd_vel_timer() { } // Ref: https://github.com/ros2/demos/blob/dashing/image_tools/src/cam2image.cpp -std::string Tracker::mat_type2encoding(int mat_type) { +std::string Tracker::mat_type2encoding(int mat_type) +{ switch (mat_type) { case CV_8UC1: return "mono8"; @@ -92,8 +95,9 @@ 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; msg.width = frame.cols; @@ -105,17 +109,15 @@ void Tracker::convert_frame_to_message(const cv::Mat &frame, msg.header.frame_id = "camera_frame"; } -void Tracker::tracking(const cv::Mat &input_frame, cv::Mat &result_frame) { - // Specific colors are extracted from the input image and converted to binary - // values. +void Tracker::tracking(const cv::Mat & input_frame, cv::Mat & result_frame) +{ + // Specific colors are extracted from the input image and converted to binary values. cv::Mat hsv; cv::cvtColor(input_frame, hsv, cv::COLOR_BGR2HSV); cv::Mat extracted_bin; - 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 + 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); // Remove noise with morphology transformation @@ -125,8 +127,7 @@ void Tracker::tracking(const cv::Mat &input_frame, cv::Mat &result_frame) { // 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_NONE); // Extracting the largest contours double max_area = 0; @@ -139,58 +140,59 @@ void Tracker::tracking(const cv::Mat &input_frame, cv::Mat &result_frame) { } } - // If the contour exists (if the object exists), find the centroid of the - // contour + // 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); + 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); 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); - 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); + 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, 0, 0), 2); } else { object_is_detected_ = false; } } -CallbackReturn Tracker::on_configure(const rclcpp_lifecycle::State &) { +CallbackReturn Tracker::on_configure(const rclcpp_lifecycle::State &) +{ RCLCPP_INFO(this->get_logger(), "on_configure() is called."); - cmd_vel_timer_ = - create_wall_timer(50ms, std::bind(&Tracker::on_cmd_vel_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 cmd_vel_timer_->cancel(); - result_image_pub_ = - create_publisher("result_image", 1); - cmd_vel_pub_ = - create_publisher("cmd_vel", 1); + 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(&Tracker::image_callback, this, std::placeholders::_1)); + "camera/color/image_raw", rclcpp::SensorDataQoS(), + std::bind(&Tracker::image_callback, this, std::placeholders::_1)); return CallbackReturn::SUCCESS; } -CallbackReturn Tracker::on_activate(const rclcpp_lifecycle::State &) { +CallbackReturn Tracker::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."); + RCLCPP_ERROR( + this->get_logger(), + "Service motor_power is not avaliable."); return CallbackReturn::FAILURE; } auto request = std::make_shared(); @@ -204,7 +206,8 @@ CallbackReturn Tracker::on_activate(const rclcpp_lifecycle::State &) { return CallbackReturn::SUCCESS; } -CallbackReturn Tracker::on_deactivate(const rclcpp_lifecycle::State &) { +CallbackReturn Tracker::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(); @@ -216,7 +219,8 @@ CallbackReturn Tracker::on_deactivate(const rclcpp_lifecycle::State &) { return CallbackReturn::SUCCESS; } -CallbackReturn Tracker::on_cleanup(const rclcpp_lifecycle::State &) { +CallbackReturn Tracker::on_cleanup(const rclcpp_lifecycle::State &) +{ RCLCPP_INFO(this->get_logger(), "on_cleanup() is called."); result_image_pub_.reset(); @@ -227,7 +231,8 @@ CallbackReturn Tracker::on_cleanup(const rclcpp_lifecycle::State &) { return CallbackReturn::SUCCESS; } -CallbackReturn Tracker::on_shutdown(const rclcpp_lifecycle::State &) { +CallbackReturn Tracker::on_shutdown(const rclcpp_lifecycle::State &) +{ RCLCPP_INFO(this->get_logger(), "on_shutdown() is called."); result_image_pub_.reset();