ROS中的信息订阅

本博客内容包括：

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;
}

代码形式没有优化，如有需要自己修改哈