Visual Operator for TurtleSim 제작

지토·2021년 11월 28일
0

ROS 로봇 프로그래밍

목록 보기
11/16

중간 대체 과제임

.
turtlebot 을 움직이게 하기 위해서는 어떤 topic 을 publish 해야 한다. (topic : linear, angular velocity)

패키지 안에는 xml 이나 CMakeList 같은 파일이 있어야 하지만 소스파일은 하나만 있으면 된다.

uvc_camera 로부터 image 를 받아와서, 패키지 내의

  • subscriber : 카메라가 날리는 이미지를 구독해야 함.
  • optical flow : 들어온 이미지에서 optical flow 를 구현해 각각의 픽셀들이 어떤 방향으로 움직이고 있는지 알아내야 함.
  • direction, velocity, management : 픽셀 단위로 방향값을 판단.
  • publisher : 이 결과값을 publish

를 이용해서

velocity 를 Gazebo 내의 TurtleSim (얘도 Subscriber) 으로 보내는 것이다.

camera

uvc 카메라 패키지의 uvc_camera_node 를 rosrun 으로 실행하면 된다. 

$ rosrun uvc_camera uvc_camera_node

실행이 잘 되는지 확인하려면 rqt 를 띄우고 topic_monitor 를 활용하면 된다.

Convert into Mat

uvc camera 를 통해 들어온 image_raw 를 Matrix 형태로 변환해야 다양한 알고리즘을 적용할 수 있다.

패키지는 /image_raw 를 subscribe 해야 한다.
(topic_exam 예제 참고)

노드들간의 메시지를 주고 받기 위한 작업 순서

1) 패키지 만들기

catkin_ws 에서 catkin_create_pkg 실행하기. 

~/catkin_ws/src$ catkin_create_pkg image_transport_ws std_msgs roscpp

2) 노드에 대한 소스코드 작성
3) 패키지 관련 정보 (xml, CMakeList) 편집

4) 빌드 파일 실행

image_raw 로부터 Mat 을 만드는 예제 1

this tutorial shows how to subscribe to images using any available transport.
By using image_transport subscriber to subscribe images, any image transport can be used at run-time.

  1. Writing a Simple Image Subscriber

image_raw 로부터 Mat 을 만드는 예제 2 (github)

패키지 내 파일

  1. launch : 실행 파일
  2. source 폴더 : 소스 파일
  3. CMakeLists.txt : 컴파일을 위한 cmake 의 규칙들을 담고 있다.
  4. package.xml : 패키지 정보와 의존성들을 담고 있다.

Mission 1

Display image in real-time using imshow

일단 catkin_ws 에서 A 라는 패키지를 catkin_create_pkg 로 만들고
소스코드를 src 폴더 내에 작성하고
CMakeList 파일을 잘 수정해주고
catkin_ws 에서 catkin_make 해주고
catkin_ws 에서 rosrun 패키지이름 소스코드이름 (cpp 빼고)

해주면됨

근데 (질문 1) image_publisher 에서 publish 하는 거 말고
그냥 subscriber 파일만 만들고 따로
rosrun uvc_camera 에서 publish 하는 image_raw 만 받아와서 imshow 해도 되나?

Mission 2

Complete optical flow and direction estimation code

Display input image with motion vectors

optical_flow.cpp 파일을 새로 만들고, 소스코드를 약간 수정했다.

그리고 CMakeList 를 수정했더니 아주 잘 동작했다.

optical flow

the pattern of arrparent motion of objects, surfaces, and edges in a visual scene caused by the relative motion between an observer and a scene.

Goal: Estimate moving direction and magnitude of each pixel using image data.

카메라 앞에서 어떤 물체를 움직일 때 픽셀 단위로 움직임의 방향과 크기를 추정하는 것.

픽셀 단위로 하면 너무 촘촘하기 때문에 10~20 픽셀 정도 사이의 간격을 두고 격자로 샘플링된 지점에서 찾는다.

optical flow 는 인접한 두 장의 영상에 나타나는 '명암 변화' 만 고려한다. 물체를 검출하고, 검출한 물체의 움직임을 반영하려는 시도를 전혀 하지 않기 떄문에 물체에 독립적인 방식이다.

t라는 순간의 영상과 짧은 시간이 흐른 후의 인접 영상이 주어졌다고 했을 때, y와 x 방향의 이동 알고리즘이 추정해야 할 모션 벡터를 v(v,u) 구하는 것이다.

이때 optical flow 에서는 brightness constancy 를 가정한다. 연속한 두 영상에 나타난 물체의 같은 점은 명암값이 같거나 비슷하다는 것이다.

Optical Flow sample code

교수님께서 배포해주신 of 코드 

#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/optflow.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/video.hpp>
#include <sys/stat.h>

using namespace cv;
using namespace std;

int main(int argc, char** argv)
{
    VideoCapture capture("../people.mp4");
    if (!capture.isOpened()){
        //error in opening the video input
        cerr << "Unable to open file!" << endl;
        return 0;
    }
    Mat frame1, prvs;
    capture >> frame1;
    cvtColor(frame1, prvs, COLOR_BGR2GRAY);
    while(true){
        Mat frame2, next;
        capture >> frame2;
        if (frame2.empty())
            break;
        cvtColor(frame2, next, COLOR_BGR2GRAY);
        Mat flow(prvs.size(), CV_32FC2);
        calcOpticalFlowFarneback(prvs, next, flow, 0.5, 3, 15, 3, 5, 1.2, 0);
        // visualization
        Mat flow_parts[2];
        split(flow, flow_parts);
        Mat magnitude, angle, magn_norm;
        cartToPolar(flow_parts[0], flow_parts[1], magnitude, angle, true);
        normalize(magnitude, magn_norm, 0.0f, 1.0f, NORM_MINMAX);
		Mat oang = angle * 3.141592 / 180.0;
        angle *= ((1.f / 360.f) * (180.f / 255.f));

        //build hsv image
        Mat _hsv[3], hsv, hsv8, bgr;
        _hsv[0] = angle;
        _hsv[1] = Mat::ones(angle.size(), CV_32F);
        _hsv[2] = magn_norm;
        merge(_hsv, 3, hsv);
        hsv.convertTo(hsv8, CV_8U, 255.0);
        cvtColor(hsv8, bgr, COLOR_HSV2BGR);
        imshow("frame2", bgr);

		// representation using vectors
		int step = 10;
		Mat img_vec = frame2;
		for (int r=0; r<angle.rows; r+=step) {
			for (int c=0; c<angle.cols; c+=step){
				float ang = oang.at<float>(r,c);
				float m = magn_norm.at<float>(r,c) * 20.0;
				Point pt1 = cv::Point(c, r);
				Point pt2 = cv::Point(c + m * cos(ang) , r + m * sin(ang));
				line(img_vec, pt1, pt2, Scalar(0, 255, 0), 1, 8, 0); 
			}
		}
        imshow("frame3", img_vec);




        int keyboard = waitKey(30);
        if (keyboard == 'q' || keyboard == 27)
            break;
        prvs = next;
    }	

	return 0;	
}

전체적인 방향을 출력하도록 수정한 코드

#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/optflow.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/video.hpp>
#include <sys/stat.h>

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
// #include <opencv2/imgproc/imgproc.hpp>
#include <sensor_msgs/image_encodings.h>
// #include <opencv2/highgui/highgui.hpp>
#include <cmath>
#include <stdio.h>

using namespace cv;
using namespace std;
double pi = M_PI;

void imageCallback(const sensor_msgs::ImageConstPtr& msg){
    try{
        cv::imshow("Mission #1", cv_bridge::toCvShare(msg, "bgr8")-> image);

    }
    catch (cv_bridge::Exception& e){
        ROS_ERROR("cannot convert");
    }
}

int main(int argc, char** argv){
    ros::init(argc, argv, "optical_flow");
    cv::VideoCapture vc(0);
    if(!vc.isOpened()) return -1;//Failed to Connect
    vc.set(CV_CAP_PROP_FRAME_WIDTH, 640);
    vc.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
    cv::Mat frame1, prvs;
    vc >> frame1;
    cvtColor(frame1, prvs, COLOR_BGR2GRAY);

    while(1){
        Mat frame2, next;
        vc >> frame2;
        if (frame2.empty())
            break;
        cvtColor(frame2, next, COLOR_BGR2GRAY);
        Mat flow(prvs.size(), CV_32FC2);
        calcOpticalFlowFarneback(prvs, next, flow, 0.5, 3, 15, 3, 5, 1.2, 0);
        Mat flow_parts[2];
        split(flow, flow_parts);
        Mat magnitude, angle, magn_norm;
        cartToPolar(flow_parts[0], flow_parts[1], magnitude, angle, true);
        normalize(magnitude, magn_norm, 0.0f, 1.0f, NORM_MINMAX);
		Mat oang = angle * 3.141592 / 180.0;
        angle *= ((1.f / 360.f) * (180.f / 255.f));

        //build hsv image
        Mat _hsv[3], hsv, hsv8, bgr;
        _hsv[0] = angle;
        _hsv[1] = Mat::ones(angle.size(), CV_32F);
        _hsv[2] = magn_norm;
        merge(_hsv, 3, hsv);
        hsv.convertTo(hsv8, CV_8U, 255.0);
        cvtColor(hsv8, bgr, COLOR_HSV2BGR);
        // imshow("Mission #2", bgr);

        int step = 10;
        int number[4] = {};
        //right, left, up, down
        Mat img_vec = frame2;
		for (int r=0; r<angle.rows; r+=step) {
			for (int c=0; c<angle.cols; c+=step){
				float ang = oang.at<float>(r,c);
				float m = magn_norm.at<float>(r,c) * 20.0;
				Point pt1 = cv::Point(c, r);
				Point pt2 = cv::Point(c + m * cos(ang) , r + m * sin(ang));
				arrowedLine(img_vec, pt1, pt2, Scalar(0, 255, 0), 1, 8, 0); 

                if((ang >= 0 && ang < pi/4) || (ang >= (pi/4)*7 && ang < pi*2)){
                    number[0] += 1;
                }
                else if(ang >= pi/4 && ang < (pi/4)*3){
                    number[2] += 1;
                }
                else if(ang >= (pi/4)*3 && ang < (pi/4)*5){
                    number[1] += 1;
                }
                else if(ang >= (pi/4)*5 && ang < (pi/4)*7){
                    number[3] += 1;
                }
			}
		}
        int max = number[0];
        for (int i=0;i<4;i++){
            if(max<number[i]){
                max = number[i];
            }
        }
        cout << max << endl;
        if (max == number[0]){
            cout << "right" << endl;
        } 
        else if (max == number[1]){
            cout << "left" << endl;
        }
        else if (max == number[2]){
            cout << "up" << endl;
        }
        else if (max == number[3]){
            cout << "down" << endl;
        }
        imshow("Mission #2", img_vec);
        int keyboard = waitKey(30);
        if (keyboard == 'q' || keyboard == 27)
            break;
        prvs = next;
    }
    return 0;
}

ang 의 값을 기준으로 방향을 나누었다.

Get Major Direction

샘플링 포인트 마다 방향과 크기가 있기 때문에, 전체적으로 어떻게 움직이는가에 대해서는 생각해 보아야 한다.
평균을 내던지 마음대로...

publisher

Goal : Publish the exact topic as turtlebot_teleop_key

turtlebot teleop key github

Mission 3

Record Video of running all packages

Turtlebot in gazebo should move according to optical flow motion

turtlebot_teleop_key

그러니까, optical flow 결과값으로 right, left 등의 결과를 출력하고 이를 message 로 Gazebo 한테 보내야 하는데
이 message 형식은 turtlebot_teleop_key 를 참고 혹은 모방해야 한다는 것이다.

그러기 위해 turtlebot_key.cpp 코드를 살펴보자.

어떤 topic 을 publish 하는지 rqt 를 통해 살펴보면,
cmd_vel 이라는 이름이고 geometry_msgs 의 twist 라는 타입으로 이루어져 있다.
이는 두 개 짜리 벡터를 변수로 가지는데 angular 하고 linear 가 있다.

소스 코드

잘 이해가 가지 않으니 따라 치면서 이해해 보아야 겠다. 

#define KEYCODE_R 0x43 

class TurtlebotTeleop{
	public:
    	TurtlebotTeleop();
        void keyLoop();
        void watchdog();
        
   	private:
    	ros::NodeHandle nh_, ph_;
        double linear_, angular_;
        ros::Time first_publish_;
        ros::Time last_publish_;
        double l_scale_, a_scale_;
        ros::Publisher vel_pub_;
        void publish(double, double);
        boost::mutex publish_mutex_;
};

TurtlebotTeleop::TurtlebotTeleop():
	ph_("~"),
    	linear_(0),
    	angular_(0),
        l_scale_(1.0);
        a_scale_(1.0);
        {
        ph_.param("scale_angular", a_scale_, a_scale_);
        ph_.param("scale_linear", l_scale_, l_scale_);
        vel_pub_=nh_.advertise<geometry_msgs::Twist>("cmd_vel",1);
        }
        
        int kfd = 0;
        struct termios cooked, raw;
        
        int main(int argc, char**argv){
        	ros::init(argc, argv, "turtlebot_teleop");
            TurtlebotTeleop turtlebot_teleop;
            ros::NodeHandle n;
            
            signal(SIGINT, quit);
            boost::thread my_thread(boost::bind(&TurtlebotTeleop::keyLoop, &turtlebot_teleop));
            ros::Timer timer
        }

되긴 하는데, 로봇이 진동함 (?)

profile
지토
이전 포스트

중간보고서

다음 포스트

SLAM_Kalman Filter

0개의 댓글