本博客內容包括：

1、創建發布節點

2、創建訂閱節點：訂閱節點訂閱兩個話題，一個持續訂閱，一個只訂閱一次資料作為初始化

（目的是為了做個記錄，很容易忘記這部分的內容，用到的時候又會花很多時間，以此記錄，）
創建功能包和節點的程序參見：功能包創建

發布節點：

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <iostream>

int main(int argc, char** argv)
{
    ros::init(argc, argv,"location_info");//節點名稱
    ros::NodeHandle nh_;  // 定義ROS句柄
    ros::Publisher msgPointPub = nh_.advertise<geometry_msgs::PointStamped>("location_info", 1000);//發布器

    geometry_msgs::PointStamped msgPointStamped;
    while(ros::ok())
    {
        msgPointStamped.point.x=10;
        msgPointStamped.point.y=10;
        msgPointStamped.point.z=10;
        ros::Time current_time = ros::Time::now();
        msgPointStamped.header.stamp = current_time;
        msgPointStamped.header.frame_id = "map";
        msgPointPub.publish(msgPointStamped);
    }
    
    return 0;
}

訂閱節點：

#include "ros/ros.h"
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include "tf/transform_broadcaster.h"
#include "tf/tf.h"
#include <geometry_msgs/Quaternion.h>
#include <nav_msgs/Odometry.h>
#include <iostream>

using namespace std;

nav_msgs::Odometry odom;
geometry_msgs::Quaternion q;//定義四元數
ros::Subscriber odom_sub;//odom的訂閱者

void odom_Callback(const nav_msgs::Odometry::ConstPtr& odom_msg)
	{
		nav_msgs::Odometry odom_msg1 = *odom_msg;
		odom.pose.pose=odom_msg1.pose.pose;
        std::cout << " [" << odom.pose.pose.orientation.w << ", " << odom.pose.pose.orientation.x << ", " 
                        << odom.pose.pose.orientation.y << ", " << odom.pose.pose.orientation.z << "]" << std::endl
                                        << std::endl;
	}
 
int main(int argc, char **argv)
{
    // 初始化ROS節點
    ros::init(argc, argv, "slo_pose_listener");
    // 創建節點句柄
    ros::NodeHandle n;
    odom_sub = n.subscribe("/odom", 1, odom_Callback);

    //方法二（這個可以，只訂閱一次資料）
    geometry_msgs::PointStamped point_vlc;
	boost::shared_ptr<geometry_msgs::PointStamped const> point_vlc_ptr;
	point_vlc_ptr = ros::topic::waitForMessage<geometry_msgs::PointStamped>("/location_info", ros::Duration(5));
	if(point_vlc_ptr != NULL)
	{
	point_vlc = *point_vlc_ptr;
	// ROS_INFO("point_vlc_x=%.5f, y=%.5f, z=%.5f", point_vlc.point.x*100, point_vlc.point.y*100, point_vlc.point.z*100);
	}
	else
	{
	ROS_INFO("no topic location/info!!");
	}


	static tf2_ros::TransformBroadcaster vlpodom_broadcaster;//tf發布器
    geometry_msgs::TransformStamped myodom_odom;//myodom_odom;
    ros::Rate loop_rate(100);

    while(ros::ok())
    {
        std::cout << " [" << point_vlc.point.x << ", "
                  << point_vlc.point.y << ", "
                  << point_vlc.point.z << "]" << std::endl;

		ros::Time current_time = ros::Time::now();
        myodom_odom.header.stamp = current_time;
        myodom_odom.header.frame_id = "vlp_odom";//"map";
        myodom_odom.child_frame_id = "odom";
        myodom_odom.transform.translation.x = point_vlc.point.x+odom.pose.pose.position.x;
        myodom_odom.transform.translation.y = point_vlc.point.y+odom.pose.pose.position.y;
        myodom_odom.transform.translation.z = point_vlc.point.z+odom.pose.pose.position.z;
        myodom_odom.transform.rotation.x = odom.pose.pose.orientation.x;//q.x;
        myodom_odom.transform.rotation.y = odom.pose.pose.orientation.y;
        myodom_odom.transform.rotation.z = odom.pose.pose.orientation.z;
        myodom_odom.transform.rotation.w = odom.pose.pose.orientation.w;

    ///發布tf
        vlpodom_broadcaster.sendTransform(myodom_odom);
        // ros::spin();
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

代碼形式沒有優化，如有需要自己修改哈