ROS学习随笔（一）

看鲁哥的程序源代码，想要学下ROS顺便改出我需要的部分。

1.图片发布器

调用语句为

rosrun dataset_publisher stereo_kitti_publisher /home/stein/05111658/ 5

读取图片路径部分为

        vector<string> vstrImageLeft;
	vector<string> vstrImageRight;
	vector<double> vTimestamps;
	LoadImages(string(argv[1]), vstrImageLeft, vstrImageRight, vTimestamps);
	const int nImages = vstrImageLeft.size();

其中vector中存储的是每张图片的绝对路径，nImages是图片总数。

	image_transport::Publisher image_pub_left;
	image_transport::Publisher image_pub_right;

	image_pub_left = it_.advertise("/camera/left/image_raw", 1);
	image_pub_right = it_.advertise("/camera/right/image_raw", 1);

上面定义了两个消息发布器，并且指定了发布频段和最大缓存数量（频道中最多保留几张图）。

	cv_bridge::CvImage cvi_l;
    	cv_bridge::CvImage cvi_r;
	cvi_l.header.frame_id = "image";
    	cvi_r.header.frame_id = "image";
	cvi_l.encoding = "bgr8";
    	cvi_r.encoding = "bgr8";

cv_bridge可以将ROS格式图片和Opencv格式互相转化。

		Mat img;
		cv::resize(imgLeft,img,Size(imgLeft.cols/2,imgLeft.rows/2));
		imshow("img",img);

运行时显示图片，除2是因为原图太大。

		cvi_l.header.stamp = time;
        		cvi_r.header.stamp = time;

		cvi_l.image = imgLeft;
        		cvi_r.image = imgRight;

		sensor_msgs::Image im_l;
		sensor_msgs::Image im_r;
		cvi_l.toImageMsg(im_l);
        		cvi_r.toImageMsg(im_r);

cv_bridge读取图片，将其转化为消息格式存入im_x，通过前面的消息发布器发布。

2.SLAM核心部分

调用语句为

roslaunch ap_slam_core ap_slam_zed.launch

这部分还没细看，rviz可以看到发布了相机、车的位姿以及轨迹，还有匹配时的所有特征点。

3.DenseMapper深度建图部分

调用语句为

roslaunch ap_dense_mapper ap_densemapper_zed.launch

我的毕设需要构建俯瞰图，所以只看如何获取的图片和位姿信息。从main函数开始。

ParamCam cam(_config_dir);

这句之前读取了相机参数。

    message_filters::Subscriber<sensor_msgs::Image> colorImgLeft_sub(nh, "/colorImgLeft_now", 1);
    message_filters::Subscriber<sensor_msgs::Image> colorImgRight_sub(nh, "/colorImgRight_now", 1);
    message_filters::Subscriber<geometry_msgs::PoseStamped> pose_sub(nh, "/camera_pose", 1);
    message_filters::Subscriber<ap_msgs::APCamInfo> apcaminfo_sub(nh, "/apcam_info", 1);

订阅器定义，指定订阅频道。

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, 
							    sensor_msgs::Image, 
						            geometry_msgs::PoseStamped, 
						            ap_msgs::APCamInfo> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), colorImgLeft_sub, 
							       colorImgRight_sub, 
							       pose_sub, 
							       apcaminfo_sub);
    sync.registerCallback(boost::bind(&GrabInfo,_1, _2, _3, _4));

这段没看太懂，感觉是定义了一个回调函数的调用方式，即产生变化就调用GrabInfo函数。

看GrabInfo里面，大概是读取了左右图、位姿和相机信息，并把bNewInfo置true.

接下来DenseMapping。大致就是用接受到的信息进行的操作。