20210804

Andrew Kyung·2021년 8월 4일

오늘의 공부

목록 보기

3/3

ROS 메세지 Graph

라즈베리파이 2대 -> TX2로 image 전송하는 시스템이다.

pub.cpp

> pub.cpp

int main(int argc, char **argv){
        ros::init(argc, argv, "image_pub2"); // PI1: image_pub1
        ros::NodeHandle nh;
        image_transport::ImageTransport it(nh);
        image_transport::Publisher pub = it.advertise("camera/image2", 1);
        // PI1: camera/image1 PI2: camera/image2


        // 4 for Yoga-Slim
        cv::VideoCapture cap(4, cv::CAP_V4L);
        if(!cap.isOpened()) return 1;
        cv::Mat frame;
        sensor_msgs::ImagePtr msg;

        ros::Rate loop_rate(5);
        while(nh.ok()){
                cap >> frame;
                imshow("test", frame); // TEST

                if(!frame.empty()){
                        std_msgs::Header header;
                        header.stamp = ros::Time::now();

                        msg = cv_bridge::CvImage(header, "bgr8", frame).toImageMsg();
                        pub.publish(msg);
                        ROS_INFO("send");
                }


                if(cv::waitKey(1) >= 0) break;
                ros::spinOnce();
                loop_rate.sleep();
        }
}

cv_bridge::CvImage(std_msg::Header(), ...) 으로 토픽을 생성하면 ROS 토픽을 생성한 시간을 보여주는 "timestamp"가 NULL로 저장이 된다.

#rostopic echo /camera/image1 > test.txt
다음 명령어로 topic 내용을 test.txt에 저장해서 볼 수 있다.

다음 사진은 ImagePtr에 대한 Structure이다.

sensor_msg의 ImagePtr에 대해 간략히 설명하면 timestamp, frameid를 저장하고 있는 Header, 높이 , 너비, 인코딩 규격, 인코딩된 데이터 값이 들어있다.
Header을 제외한 나머지 부분은 cv_bridge::CvImage에서 자동으로 변환해주지만, ImagePtr에 Header에 대한 정보도 추가하고 싶으면 header.stamp에 ros::Time::now() 값을 넣으면 timestamp 값을 저장할 수 있다.

sub.cpp

> sub.cpp
void callback(const sensor_msgs::ImageConstPtr& image1, const sensor_msgs::ImageConstPtr& image2)
{
  ROS_INFO("success");
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "image_listener");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);

  image_transport::SubscriberFilter sub1(it, "camera/image1", 1);
  image_transport::SubscriberFilter sub2(it, "camera/image2", 1);

  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> SyncPolicy;
  message_filters::Synchronizer<SyncPolicy> sync(SyncPolicy(10), sub1, sub2);

  sync.registerCallback(boost::bind(&callback, _1, _2));
  std::cout << "output\n";
  ros::spin();
}

(#8) ROS 노드 초기화
(#9) 노드의 sub-component를 제어할 수 있는 NodeHandler 선언
(#10) ImageTransport 함수를 통해 NodeHandler을 Wrapping 한다.
(#12 ~ #13) "camera/image" Topic에 대한 subscriber 선언한다.

http://docs.ros.org/en/noetic/api/image_transport/html/classimage__transport_1_1SubscriberFilter.html
첫번째 파라미터는 ImageTransport 함수를 받는다.
두번째 파라미터는 topic 명칭을 받는다. (해당 예제에서는 camera/image)
세번째 파라미터는 queue size를 int로 받는다.
마지막 파라미터는 이미제에 대한 정보를 받는다. ( raw(default), theora, compressed ...)

(#15) message_filter::sync_policies 는 ApproximateTime이나 ExactTime 등 동기화(sync)에 대한 설정을 할 수 있다.
ExactTime의 경우, timestamp과 정확히 같을 때만 동기화한다. (주로 한 기기에서 나온 정보를 동기화 할때 사용되는 거 같음)

http://wiki.ros.org/message_filters/ApproximateTime
ApproximateTime은 비슷한 주기를 가진 Topic들을 동기화한다.
만약에 topic들의 주기가 다르면 Topic이 생략되는 경우도 있다.
(#16) message_filters::Synchronizer

http://docs.ros.org/en/noetic/api/message_filters/html/c++/classmessage__filters_1_1Synchronizer.html#a4a9dc3f5236f36fc2b1fb74a5b0872d7
첫번째 파라미터는 아까 설정한 sync_policies와 동기화할 queue 개수를 int로 받는다.
두번째 파라미터부터는 동기화할 Topic들을 받는다.