利用ros的发布订阅机制来传输建图生成的yaml文件和pgm文件（实现地图共享）

传输yaml文件

由于采用C++编写需要安装什么解析yaml的包yaml-cpp。我一整个周日都花费在安装这个上面了，也没按好，所以采用的python。（简直不要太好用）
这时我们的yaml文件内容：

image: turtlebot_test_map.pgm
resolution: 0.050000
origin: [-13.800000, -15.400000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

然后根据内容我们确定了自定义消息的格式：

string image
float64 resolution
float64 X
float64 Y
float64 W
int32 negate
float64 occupied_thresh
float64 free_thresh

废话不多说，我们先整上代码。

首先是发布端：talker：

#!/usr/bin/env python
# _*_ coding:utf-8 _*_
import sys
import rospy
import yaml
from handle_yaml.msg import yamlMsg

def send_yaml():
    rospy.init_node("read_yaml_talker",anonymous=True)
    pub=rospy.Publisher("handle_yaml",yamlMsg,queue_size=50)
    f=open('/home/daniel/map/turtlebot_test_map.yaml')
    content=yaml.load(f)
    print(content)
    temp=yamlMsg()
    temp.image=content['image']
    temp.resolution=content['resolution']
    temp.X=content['origin'][0]
    temp.Y=content['origin'][1]
    temp.W=content['origin'][2]
    temp.negate=content['negate']
    temp.occupied_thresh=content['occupied_thresh']
    temp.free_thresh=content['free_thresh']
    rate=rospy.Rate(10)
    #count=0
    while not rospy.is_shutdown():
        #if(count==0):
        print(temp)
        pub.publish(temp)
        print("already sent")
         #   count=1
        rate.sleep()

if __name__=='__main__':
    try:
        send_yaml()
    except rospy.ROSInterruptException:
        pass

然后我们对代码进行解析：

import sys
import rospy
import yaml
from handle_yaml.msg import yamlMsg

先导入rospy模块，yaml模块 和自定义的消息模块

 f=open('/home/daniel/map/turtlebot_test_map.yaml')
 content=yaml.load(f)

这段代码是打开我们指定路径的yaml文件，然后将其加载成字典

    temp=yamlMsg()
    temp.image=content['image']
    temp.resolution=content['resolution']
    temp.X=content['origin'][0]
    temp.Y=content['origin'][1]
    temp.W=content['origin'][2]
    temp.negate=content['negate']
    temp.occupied_thresh=content['occupied_thresh']
    temp.free_thresh=content['free_thresh']

这里是将字典的值赋给我们自定义消息类的对象。然后之后将自定义消息对象进行发布。

pub.publish(temp)

然后将消息进行发布

listener：

#!/usr/bin/env python
# _*_ coding:utf-8 _*_
__author__ = 'Anthony'
import sys
import rospy
import yaml
import os
from handle_yaml.msg import yamlMsg

def callback(Msg):
    yaml_dict={}
    yaml_dict['image']=Msg.image
    yaml_dict['resolution']=Msg.resolution
    #yaml_dict['origin'][0]=Msg.X
    #yaml_dict['origin'][1]=Msg.Y
    #yaml_dict['origin'][2]=Msg.W
    yaml_dict['origin']=[Msg.X,Msg.Y,Msg.W]
    yaml_dict['negate']=Msg.negate
    yaml_dict['occupied_thresh']=Msg.occupied_thresh
    yaml_dict['free_thresh']=Msg.free_thresh
    curpath=os.path.dirname(os.path.realpath(__file__))
    yamlpath=os.path.join(curpath,"test.yaml")
    print('the path is:%s',yamlpath)
    # write yaml file
    with open(yamlpath,'w',1)as f:
        yaml.dump(yaml_dict,f)
    
def rec_yaml():
    rospy.init_node('write_yaml_listener',anonymous=True)
    rospy.Subscriber('handle_yaml',yamlMsg,callback)
    rospy.spin()

if __name__=='__main__':
    try:
        rec_yaml()
    except rospy.ROSInterruptException:
        pass

下面解析这段代码：

    yaml_dict['image']=Msg.image
    yaml_dict['resolution']=Msg.resolution
    #yaml_dict['origin'][0]=Msg.X
    #yaml_dict['origin'][1]=Msg.Y
    #yaml_dict['origin'][2]=Msg.W
    yaml_dict['origin']=[Msg.X,Msg.Y,Msg.W]
    yaml_dict['negate']=Msg.negate
    yaml_dict['occupied_thresh']=Msg.occupied_thresh
    yaml_dict['free_thresh']=Msg.free_thresh

这个是将订阅过来的消息付给我们建立的字典（注意注释这样不行）必须是下面那行代码，才能可以（这又坑了我半天时间55555555555555）唉，不熟悉py的原因，

 curpath=os.path.dirname(os.path.realpath(__file__))
 yamlpath=os.path.join(curpath,"test.yaml")

第一行是获取当前运行文件的路径，然后将我们的test.yaml 文件加入到当前路径里面

with open(yamlpath,'w',1)as f:
        yaml.dump(yaml_dict,f)

将字典写到test.yaml文件中,大功告成！！！！

传输地图文件pgm

我们使用image_transport发布者，它可以发布单一图像或多个图像。
这个我找了github上的一个源码。链接是 https://github.com/tzutalin/ros_sample_image_transport
然后注释掉我们不需要的功能就ok了。

发布端：

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace cv;

static const std::string IMAGE_PATH = "/home/daniel/map/turtlebot_test_map.pgm";
static const std::string TOPIC_NAME = "camera/rgb/image";

int publishImage(std::string filepath)
{
    Mat image;
    image = imread(filepath, CV_LOAD_IMAGE_COLOR);   // Read the file
    std::cout << "Path " << filepath << std::endl;
    if(!image.data)                              // Check for invalid input
    {
        std::cout << "Could not open or find the image" << std::endl ;
        return -1;
    }

    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    image_transport::Publisher pub = it.advertise(TOPIC_NAME, 1);
    sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
    ros::Rate loop_rate(5);

    while (nh.ok()) {
        pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
    }
}

int publishImageWithoutImage_transport()
{
    ROS_INFO("Topic : %s", TOPIC_NAME.c_str());
    ROS_INFO("IMAGE PATH : %s", IMAGE_PATH.c_str());
    ros::NodeHandle nh;
    std::string image_path = IMAGE_PATH;
    cv_bridge::CvImage cv_image;
    cv_image.image = cv::imread(image_path, CV_LOAD_IMAGE_COLOR);
    cv_image.encoding = "bgr8";
    sensor_msgs::Image ros_image;
    cv_image.toImageMsg(ros_image);

    ros::Publisher pub = nh.advertise<sensor_msgs::Image>(TOPIC_NAME, 1);
    ros::Rate loop_rate(5);

    while (nh.ok())
    {
        pub.publish(ros_image);
        ros::spinOnce();
        loop_rate.sleep();
    }

}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "image_transport_publisher");
    publishImageWithoutImage_transport();
    return 0;
}

这里我们仅仅用了 int publishImageWithoutImage_transport() 这个函数。没有采用Image_transport来传输

订阅端：

/*
 * image_listener.cpp
 *
 *  Created on: Apr 30, 2015
 *      Author: darrenl
 */
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

//static const std::string TOPIC_NAME = "camera/rgb/image_raw";
static const std::string DEPTH_TOPIC_NAME = "camera/depth/image_raw";
static const std::string TOPIC_NAME = "camera/rgb/image";

void imageCallback(const sensor_msgs::ImageConstPtr& msg) {
    try {
        cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
        cv::imwrite("/home/daniel/map/test.pgm", cv_ptr->image);
      //  cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
        cv::waitKey(30);
    } catch (cv_bridge::Exception& e) {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.",
                msg->encoding.c_str());
    }
}

void imageDepthCallback(const sensor_msgs::ImageConstPtr& msg) {
    try {
        // Save image
        cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "16UC1");
        cv::Mat mat = cv_ptr->image;
        cv::Mat beConvertedMat(mat.rows, mat.cols, CV_8UC4, mat.data); // provide different view of m1 data
        cv::imwrite("rgbd.bmp", beConvertedMat);
        // Show image
        cv::imshow("depthview", cv_bridge::toCvShare(msg, "16UC1")->image);
        cv::waitKey(30);
    } catch (cv_bridge::Exception& e) {
        ROS_ERROR("Could not convert from '%s' to '16UC1'.",
                msg->encoding.c_str());
    }
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "image_transport_subscriber");
    ros::NodeHandle nh;
   // cv::namedWindow("view");
   // cv::namedWindow("depthview");
   // cv::startWindowThread();
    image_transport::ImageTransport it(nh);
    image_transport::Subscriber sub = it.subscribe(TOPIC_NAME, 1,
            imageCallback);
  //  image_transport::Subscriber sub_depth = it.subscribe(DEPTH_TOPIC_NAME, 1,
  //          imageDepthCallback);
    ros::spin();
 //   cv::destroyWindow("view");
 //   cv::destroyWindow("depthview");
    ros::shutdown();
    return 0;
}

注意源码中把订阅端和发布端的主题topic改成一样的

//static const std::string TOPIC_NAME = "camera/rgb/image_raw";
static const std::string TOPIC_NAME = "camera/rgb/image";

然后我们不需要imageDepthCallback这个回调函数 在主函数中将其注释掉

 image_transport::Subscriber sub = it.subscribe(TOPIC_NAME, 1,
            imageCallback);
  //  image_transport::Subscriber sub_depth = it.subscribe(DEPTH_TOPIC_NAME, 1,
  //          imageDepthCallback);

我们只需要保存文件，也不需要显示出来，也将其注释掉

  cv::imwrite("/home/daniel/map/test.pgm", cv_ptr->image);
  //  cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);

ok，大功告成。
然后接受来的地图文件就被保存到 /home/daniel/map/test.pgm 这个文件目录下了。