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 
#include 
#include 
#include 
#include 
#include 
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_を採用していませんトランスポートで送信
サブスクリプション:

/*
 * image_listener.cpp
 *
 *  Created on: Apr 30, 2015
 *      Author: darrenl
 */
#include 
#include 
#include 
#include 

//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というファイルディレクトリの下にあります.