ROS2 25 LINE TRACER & LANE FOLLOWER 1

[정용규]

pub

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89

#include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/compressed_image.hpp" #include "cv_bridge/cv_bridge.h" #include "opencv2/opencv.hpp" #include "geometry_msgs/msg/vector3.hpp" //#include "dxl/dxl.hpp" #include <memory> #include <chrono> #include <functional> #include <thread> #include <chrono> #include<math.h> #define SPEED 100 using namespace std::chrono_literals; using std::placeholders::_1; class dd : public rclcpp::Node{     rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr sub;     rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr pub;     geometry_msgs::msg::Vector3 vel;    // rclcpp::TimerBase::SharedPtr timer_;     void subcallback(const sensor_msgs::msg::CompressedImage::SharedPtr msg){         cv::Mat frame, stats, centroids, labels;         frame = cv::imdecode(cv::Mat(msg->data),  cv::IMREAD_COLOR);         cv::imshow("wsl",frame);         cv::waitKey(1);         RCLCPP_INFO(this->get_logger(), "Received Image : %s,%d,%d", msg->format.c_str(),frame.rows,frame.cols);         frame = frame(cv::Rect(cv::Point(0,frame.rows/4*3),cv::Point(frame.cols,frame.rows)));         static cv::Point po(frame.cols/2,frame.rows/2);         cv::cvtColor(frame, frame, cv::COLOR_BGR2GRAY);         cv::threshold(frame, frame, 0, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);         int lable =  cv::connectedComponentsWithStats(frame, labels, stats, centroids);         cv::cvtColor(frame, frame, cv::COLOR_GRAY2BGR);         int index,a=1;         static bool b = true;         double mindistance;         for(int i = 1;i<lable;i++){             if(stats.at<int>(i,4)<50){                 a++;                 continue;             }             if(i == a){                 index = i;                 mindistance = sqrt(pow((po.x - centroids.at<double>(i,0)),2)+pow((po.y - centroids.at<double>(i,1)),2));             }             else if(sqrt(pow((po.x-centroids.at<double>(i,0)),2)+pow((po.y- centroids.at<double>(i,1)),2))<mindistance){                 mindistance = sqrt(pow((po.x-centroids.at<double>(i,0)),2)+pow((po.y- centroids.at<double>(i,1)),2));                 index = i;             }          }         if(po.y>frame.rows-35){                 b = false;             }         if(mindistance < 100){             b = true;             }          for(int i = 1;i<lable;i++){                 cv::Scalar sc = i == index && b ? cv::Scalar(0,0,255):cv::Scalar(255,0,0);                 if(stats.at<int>(i,4)<50) sc = cv::Scalar(0,255,255);                 cv::rectangle(frame,cv::Rect(stats.at<int>(i,0),stats.at<int>(i,1),stats.at<int>(i,2),stats.at<int>(i,3)),sc);                 cv::rectangle(frame,cv::Rect(centroids.at<double>(i,0),centroids.at<double>(i,1),3,3),sc);             }              cv::rectangle(frame,cv::Rect(po.x,po.y,3,3),cv::Scalar(0,255,0));             if(b){                     po = cv::Point(centroids.at<double>(index,0),centroids.at<double>(index,1));             }             cv::rectangle(frame,cv::Rect(po.x,po.y,3,3),cv::Scalar(0,255,0));             vel.x = SPEED +po.x;             vel.y = -1*SPEED +po.x;             pub->publish(vel);         }     public:     dd(std::string s): Node(s){         vel.x = 0;         vel.y = 0;         vel.z = 0;         pub = this->create_publisher<geometry_msgs::msg::Vector3>("topic_dxlpub", rclcpp::QoS(rclcpp::KeepLast(10)));         sub = this->create_subscription<sensor_msgs::msg::CompressedImage>("topic_campub", rclcpp::QoS(             rclcpp::KeepLast(10)).best_effort(),std::bind(&dd::subcallback, this, _1));      //   timer_ = this->create_wall_timer(40ms,std::bind(&dd::pubcallback ,this));     } }; int main(int argc, char * argv[]) {     rclcpp::init(argc, argv);     auto node = std::make_shared<dd>("node_dxlpub");     rclcpp::spin(node);     rclcpp::shutdown();     return 0; } Colored by Color Scripter

cs

sub

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70

#include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/compressed_image.hpp" #include "cv_bridge/cv_bridge.h" #include "opencv2/opencv.hpp" #include "geometry_msgs/msg/vector3.hpp" #include "dxl/dxl.hpp" #include <memory> #include <chrono> #include <functional> #include <thread> using namespace std::chrono_literals; using std::placeholders::_1; std::string src = "nvarguscamerasrc sensor-id=0 ! \     video/x-raw(memory:NVMM), width=(int)640, height=(int)360, \     format=(string)NV12 ! nvvidconv flip-method=0 ! video/x-raw, \     width=(int)640, height=(int)360, format=(string)BGRx ! \     videoconvert ! video/x-raw, format=(string)BGR ! appsink";    class dd : public rclcpp::Node{     rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr pub;     rclcpp::Subscription<geometry_msgs::msg::Vector3>::SharedPtr sub;     cv::VideoCapture cap;     rclcpp::TimerBase::SharedPtr t;     std_msgs::msg::Header h;      Dxl dxl;     void mysub_callback(     const geometry_msgs::msg::Vector3::SharedPtr msg){     RCLCPP_INFO(this->get_logger(), "Received message: %lf, %lf", msg->x, msg->y);     dxl.setVelocity(static_cast<int>(msg->x), static_cast<int>(msg->y)); } void mypub_callback() {     cv::Mat img;     cap >> img;     if (img.empty()) {  RCLCPP_WARN(this->get_logger(), "Empty frame captured!"); return;}      RCLCPP_INFO(this->get_logger(),"image pub");     auto msg = cv_bridge::CvImage(h, "bgr8", img).toCompressedImageMsg();     pub->publish(*msg);  }     public:     dd(std::string s = "dd"):Node(s){         if (!dxl.open()) {             RCLCPP_ERROR(this->get_logger(), "dynamixel open error ");             rclcpp::shutdown();     }         cap  = cv::VideoCapture("/home/jetson/ros2_ws/src/dxl/dd.mp4");         if(!cap.isOpened()){             RCLCPP_INFO(this->get_logger(),"에러");             rclcpp::shutdown();         }         sub = this->create_subscription<geometry_msgs::msg::Vector3>("topic_dxlpub",              rclcpp::QoS(rclcpp::KeepLast(10)), std::bind(&dd::mysub_callback,this,_1));           pub = this->create_publisher<sensor_msgs::msg::CompressedImage>(             "topic_campub", rclcpp::QoS(rclcpp::KeepLast(10)).best_effort());         t = this->create_wall_timer(40ms, std::bind(&dd::mypub_callback,this));     }     ~dd(){         dxl.close();     } }; int main(int argc, char* argv[]) {     rclcpp::init(argc, argv);     auto node = std::make_shared<dd>("dd");     rclcpp::spin(node);      rclcpp::shutdown();     return 0; }   Colored by Color Scripter

cs

cmake

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43

cmake_minimum_required(VERSION 3.16.3) project(dxl)   # Default to C++14 if(NOT CMAKE_CXX_STANDARD)   set(CMAKE_CXX_STANDARD 14) endif()   if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")   add_compile_options(-Wall -Wextra -Wpedantic) endif()   # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(OpenCV REQUIRED) find_package(cv_bridge REQUIRED)   include_directories(include) include_directories(/usr/local/include/dynamixel_sdk)   # Build add_executable(pub src/pub.cpp src/dxl.cpp ) ament_target_dependencies(pub rclcpp geometry_msgs cv_bridge sensor_msgs OpenCV)  target_link_libraries(pub dxl_x64_cpp) #target_include_directories(pub PUBLIC /usr/local/include/dynamixel_sdk)   add_executable(sub src/sub.cpp src/dxl.cpp) ament_target_dependencies(sub rclcpp geometry_msgs cv_bridge sensor_msgs OpenCV) target_link_libraries(sub dxl_x64_cpp) #target_include_directories(sub PUBLIC /usr/local/include/dynamixel_sdk)   install(TARGETS   pub   sub   DESTINATION lib/${PROJECT_NAME})   ament_package()       Colored by Color Scripter

cs

pub

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90

#include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/compressed_image.hpp" #include "cv_bridge/cv_bridge.h" #include "opencv2/opencv.hpp" #include "geometry_msgs/msg/vector3.hpp" //#include "dxl/dxl.hpp" #include <memory> #include <chrono> #include <functional> #include <thread> #include <chrono> #include<math.h> #define SPEED 150 using namespace std::chrono_literals; using std::placeholders::_1; class dd : public rclcpp::Node{     rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr sub;     rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr pub;     geometry_msgs::msg::Vector3 vel;    // rclcpp::TimerBase::SharedPtr timer_;     void subcallback(const sensor_msgs::msg::CompressedImage::SharedPtr msg){         cv::Mat frame, stats, centroids, labels;         frame = cv::imdecode(cv::Mat(msg->data),  cv::IMREAD_COLOR);         cv::imshow("wsl",frame);         cv::waitKey(1);         RCLCPP_INFO(this->get_logger(), "Received Image : %s,%d,%d", msg->format.c_str(),frame.rows,frame.cols);         frame = frame(cv::Rect(cv::Point(0,frame.rows/4*3),cv::Point(frame.cols,frame.rows)));         static cv::Point po(frame.cols/2,frame.rows/2);         cv::cvtColor(frame, frame, cv::COLOR_BGR2GRAY);         cv::threshold(frame, frame, 0, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);         int lable =  cv::connectedComponentsWithStats(frame, labels, stats, centroids);         cv::cvtColor(frame, frame, cv::COLOR_GRAY2BGR);         int index,a=1;         static bool b = true;         double mindistance;         for(int i = 1;i<lable;i++){             if(stats.at<int>(i,4)<50){                 a++;                 continue;             }             if(i == a){                 index = i;                 mindistance = sqrt(pow((po.x - centroids.at<double>(i,0)),2)+pow((po.y - centroids.at<double>(i,1)),2));             }             else if(sqrt(pow((po.x-centroids.at<double>(i,0)),2)+pow((po.y- centroids.at<double>(i,1)),2))<mindistance){                 mindistance = sqrt(pow((po.x-centroids.at<double>(i,0)),2)+pow((po.y- centroids.at<double>(i,1)),2));                 index = i;             }          }         if(po.y>frame.rows-35){                 b = false;             }         if(mindistance < 100){             b = true;             }          for(int i = 1;i<lable;i++){                 cv::Scalar sc = i == index && b ? cv::Scalar(0,0,255):cv::Scalar(255,0,0);                 if(stats.at<int>(i,4)<50) sc = cv::Scalar(0,255,255);                 cv::rectangle(frame,cv::Rect(stats.at<int>(i,0),stats.at<int>(i,1),stats.at<int>(i,2),stats.at<int>(i,3)),sc);                 cv::rectangle(frame,cv::Rect(centroids.at<double>(i,0),centroids.at<double>(i,1),3,3),sc);             }              cv::rectangle(frame,cv::Rect(po.x,po.y,3,3),cv::Scalar(0,255,0));             if(b){                     po = cv::Point(centroids.at<double>(index,0),centroids.at<double>(index,1));             }             cv::rectangle(frame,cv::Rect(po.x,po.y,3,3),cv::Scalar(0,255,0));             vel.x = SPEED - ((frame.cols/2 - po.x)/5);             vel.y = -1*SPEED - ((frame.cols/2 - po.x)/5);             cv::imshow("output",frame);             pub->publish(vel);         }     public:     dd(std::string s): Node(s){         vel.x = 0;         vel.y = 0;         vel.z = 0;         pub = this->create_publisher<geometry_msgs::msg::Vector3>("topic_dxlpub", rclcpp::QoS(rclcpp::KeepLast(10)));         sub = this->create_subscription<sensor_msgs::msg::CompressedImage>("topic_campub", rclcpp::QoS(             rclcpp::KeepLast(10)).best_effort(),std::bind(&dd::subcallback, this, _1));      //   timer_ = this->create_wall_timer(40ms,std::bind(&dd::pubcallback ,this));     } }; int main(int argc, char * argv[]) {     rclcpp::init(argc, argv);     auto node = std::make_shared<dd>("node_dxlpub");     rclcpp::spin(node);     rclcpp::shutdown();     return 0; } Colored by Color Scripter

cs

sub

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67

#include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/compressed_image.hpp" #include "cv_bridge/cv_bridge.h" #include "opencv2/opencv.hpp" #include "geometry_msgs/msg/vector3.hpp" #include "dxl/dxl.hpp" #include <memory> #include <chrono> #include <functional> #include <thread> using namespace std::chrono_literals; using std::placeholders::_1; std::string src = "nvarguscamerasrc sensor-id=0 ! \     video/x-raw(memory:NVMM), width=(int)640, height=(int)360, \     format=(string)NV12 ! nvvidconv flip-method=0 ! video/x-raw, \     width=(int)640, height=(int)360, format=(string)BGRx ! \     videoconvert ! video/x-raw, format=(string)BGR ! appsink";    class dd : public rclcpp::Node{     rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr pub;     rclcpp::Subscription<geometry_msgs::msg::Vector3>::SharedPtr sub;     cv::VideoCapture cap;     rclcpp::TimerBase::SharedPtr t;     std_msgs::msg::Header h;      Dxl dxl;     void mysub_callback(     const geometry_msgs::msg::Vector3::SharedPtr msg){     RCLCPP_INFO(this->get_logger(), "Received message: %lf, %lf", msg->x, msg->y);     dxl.setVelocity(static_cast<int>(msg->x), static_cast<int>(msg->y)); } void mypub_callback() {     cv::Mat img;     cap >> img;     if (img.empty()) {  RCLCPP_WARN(this->get_logger(), "Empty frame captured!"); return;}      RCLCPP_INFO(this->get_logger(),"image pub");       auto msg = cv_bridge::CvImage(h, "bgr8", img).toCompressedImageMsg();     pub->publish(*msg);  }     public:     dd(std::string s = "dd"):Node(s){         if (!dxl.open()) {             RCLCPP_ERROR(this->get_logger(), "dynamixel open error ");             rclcpp::shutdown();     }         cap  = cv::VideoCapture(src, cv::CAP_GSTREAMER);         sub = this->create_subscription<geometry_msgs::msg::Vector3>("topic_dxlpub",              rclcpp::QoS(rclcpp::KeepLast(10)), std::bind(&dd::mysub_callback,this,_1));           pub = this->create_publisher<sensor_msgs::msg::CompressedImage>(             "topic_campub", rclcpp::QoS(rclcpp::KeepLast(10)).best_effort());         t = this->create_wall_timer(40ms, std::bind(&dd::mypub_callback,this));     }     ~dd(){         dxl.close();     } }; int main(int argc, char* argv[]) {     rclcpp::init(argc, argv);     auto node = std::make_shared<dd>("dd");     rclcpp::spin(node);      rclcpp::shutdown();     return 0; }   Colored by Color Scripter

cs

cmake

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43

cmake_minimum_required(VERSION 3.16.3) project(dxl)   # Default to C++14 if(NOT CMAKE_CXX_STANDARD)   set(CMAKE_CXX_STANDARD 14) endif()   if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")   add_compile_options(-Wall -Wextra -Wpedantic) endif()   # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(OpenCV REQUIRED) find_package(cv_bridge REQUIRED)   include_directories(include) include_directories(/usr/local/include/dynamixel_sdk)   # Build add_executable(pub src/pub.cpp src/dxl.cpp ) ament_target_dependencies(pub rclcpp geometry_msgs cv_bridge sensor_msgs OpenCV)  target_link_libraries(pub dxl_x64_cpp) #target_include_directories(pub PUBLIC /usr/local/include/dynamixel_sdk)   add_executable(sub src/sub.cpp src/dxl.cpp) ament_target_dependencies(sub rclcpp geometry_msgs cv_bridge sensor_msgs OpenCV) target_link_libraries(sub dxl_x64_cpp) #target_include_directories(sub PUBLIC /usr/local/include/dynamixel_sdk)   install(TARGETS   pub   sub   DESTINATION lib/${PROJECT_NAME})   ament_package()       Colored by Color Scripter

cs

[Sungryul Lee]

- 패키지 생성부터 실행까지 모든 과정의 실행결과를 첨부할것
- ros 기본 명령어로 실행결과 첨부할 것(topic info, hz, bw, echo 등)
- robot view 화면과 영상처리결과를 동영상으로 저장하고 첨부할것