ROS教程：同时发布与订阅，海龟圆周运动同时终端打印位置信息

**

创建一个节点，同时实现订阅与发布

==以turtlesim为例，创建一个发布topic控制海龟圆周运动==
==同时订阅海龟/turtle1/pose话题，终端打印pose信息==
==再新建一个/vel_info话题，发布速度信息==
==新建一个订阅者，订阅刚刚发布的速度信息==

都在同一个节点里实现

#include "ros/ros.h"
#include <iostream>
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
#include <turtlesim/Pose.h>
#include <ros/console.h>

using namespace std;
void turtle1posecallback(const turtlesim::Pose& pose)
{
    bool FLAG;
    int x = (int) pose.x;
    int y = (int) pose.y;
    if(x > 0 && y > 0 && x < 11 && y < 11)
       FLAG = true;
    else
       FLAG = false;
    if (FLAG)
    {
       ROS_INFO_STREAM(std::setprecision(2) << std::fixed << endl << "x:" << pose.x << ", y:" << pose.y << "\ntheta:" << pose.theta <<
       endl << "linear_velocity:" << pose.linear_velocity << ",angular_velocity" << pose.angular_velocity << endl;
       );

    }
    else
    {
        ROS_INFO_STREAM("DATA is wrong...\n");
    }
}

 void vel_infocallback(const std_msgs::String::ConstPtr vel)
 {
//    ROS_INFO("velocity info:\n%s", vel->data.c_str());
 }

int main(int argc, char **argv)
{
    ros::init(argc, argv, "turtle1_control");

    ros::NodeHandle nh;
// 创建一个Publisher，向topic为/turtle1/cmd_vel，消息类型为geometry_msgs::Twist，队列长度10
    ros::Publisher ves_pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
// 创建一个Publisher 发布速度信息
    ros::Publisher info_pub = nh.advertise<std_msgs::String>("vel_info",10);

// 创建一个Subscriber，订阅pose信息
    ros::Subscriber ves_sub = nh.subscribe("/turtle1/pose", 10, &turtle1posecallback);
// 创建一个Subscriber，订阅自己发布的速度信息
    ros::Subscriber vel_sub = nh.subscribe("/vel_info", 10, &vel_infocallback);

    ros::Rate loop_rate(5);// 设置发布的频率

    while (ros::ok())
    {
        //画圆
        geometry_msgs::Twist vel;
        geometry_msgs::Vector3 linear;
        linear.x = 0.5;
        linear.y = 0;
        linear.z = 0;

        geometry_msgs::Vector3 angular;
        angular.x = 0;
        angular.y = 0;
        // angular.z = 0;//直行
        angular.z=-0.5;

        vel.linear = linear;
        vel.angular = angular;

        ves_pub.publish(vel);

        //准备发布速度信息
        std_msgs::String msg;
        stringstream ss;
        ss << vel;
        msg.data = ss.str();
        info_pub.publish(msg);

        ros::spinOnce();//这句就是同时发布节点和订阅节点的关键
        // loop_rate.sleep(); // 按照循环频率延时
        
    }
    
    return 0;
}

• 注：要显示打印输出位置信息的时候需要屏蔽掉速度打印输出，可能是因为频率的关系， 有时间再看看怎么回事

写一个.launch文件，一键启动turtlesim：

  1 <launch>
  2 
  3            <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node">
  4            </node>
  5 
  6            <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key">
  7            </node>
  8  </launch>