ROS机器人操作系统开发视频教程进阶-ros节点发布订阅（五）

五、ros节点发布和订阅

1、ROS节点编程
使用robware开发环境，创建节点框架
打开开发环境robot_1
然后，右键src-新建ROS包-命名为test-右键新建的test-新建C++ROS节点-节点名称为test
-自动生成文件代码框架
订阅test_sub.cpp文件代码及解释如下：

clude "ros/ros.h"
#include "std_msgs/String.h"

void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
	ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char *argv[])
{

	ros::init(argc, argv, "test_sub");
	#把进程的参数导入，可用可不用，test是节点的名称，这里不能重复，否则不能运行，
	#注意这里我们做了修改，默认是节点名称，但是发布和订阅不能相同，所以做了修改
	
	ros::NodeHandle n;
	#创建了一个ros节点的句柄
	
	ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
	#创建了一个订阅者，固定订阅者创建函数，chatter是话题消息名称，1000是消息缓冲数量，
	#chatterCalllback接收到订阅消息后用什么函数处理，就是回调函数
	
	ros::spin();
	#接受ctrl+c停止消息，也就是执行到这里会卡在这里，直到你ctrl+c停止消息
	
	return 0;
}

发布test_pub.cpp文件代码及解释如下：

#include "ros/ros.h"
#include "std_msgs/String.h"

#include <sstream>
#发布者没有回调函数，通过while循环周期或者不是周期发布消息

int main(int argc, char *argv[])
{
	ros::init(argc, argv, "test_pub");
	#节点名称，此处也做了修改，原本默认是节点名称text
	
	ros::NodeHandle n;
	#创建句柄
	
	ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
	#创建一个发布者，发布消息的格式是String，消息话题名字chatter，话题必须一样，1000个缓冲
	
	ros::Rate loop_rate(10);
	#循环的频率，这里是10Hz
	
	int count = 0;
	while (ros::ok())
	{
		std_msgs::String msg;
		创建了一个String的消息msg
		
		std::stringstream ss;	
		#又创建了一个stringstream的消息ss
		
		ss << "hello world " << count;
		#赋值给ss

		msg.data = ss.str();
		#ss转化为str字符串，放到msg里去

		ROS_INFO("%s", msg.data.c_str());

		chatter_pub.publish(msg);
		#把msg通过publish接口发布出去

		ros::spinOnce();
		#这是允许你停止或者回调的函数

		loop_rate.sleep();
		#休眠一会，精确计时，保证一个循环10Hz
		
		++count;
	}

	return 0;
}

然后我们使用Debug编译一下，通过后可以尝试下面的运行
然后上节我们把主机设置成了小车，如果想要在PC端运行，还需要把master改回PC端，PC端运行：

gedit .bahsrc

然后把上节加上的两句注释如下：

#export ROS_MASTER_URI=http://192.168.1.101:11311
#export ROS_HOSTNAME=192.168.1.100

然后需要所有的窗口全部关闭，重新打开才会执行新的配置，包括终端和robware
运行程序，打开三个终端，首先在一个终端执行：

roscore

然后，再在一个终端启动发布者，因为这个节点没有写launch文件，所以我们需要用rosrun执行：

rosrun test test_pub 

然后，在另一个终端执行，订阅节点：

rosrun test test_sub 

如果执行报错：

[ WARN] [1614497226.944395160]: Shutdown request received.
[ WARN] [1614497226.944475100]: Reason given for shutdown: [new node registered with same name]

说明两个cpp文件节点名称未作修改，修改后重新编译一下即可，再按照上述执行即可
此时，也可以看下节点的的结构：

rqt_graph 

可以看到节点结构，这里也可以参考发布和订阅节点

2、发布订阅TF消息
这一节需要首先完成第四节教程TF空间描述和变换

我们先来了解一下TF功能包API：

广播TF变换
• tf::TransformBroadcaster();
创建一个TF的广播对象
• void sendTransform(const StampedTransform & transform);

• void sendTransform(const geometry_msgs::TransformStamped & transform);


测试接收到的TF变换
• tf::TransformListener::canTransform（ ……）

• tf::TransformListener::waitForTransform(……)


接收并转换TF数据
• tf::TransformListener::lookupTransform (……) 变换2个坐标

• tf::TransformListener::transformDATATYPE(…...) 转换数据

下面是编写代码，打开robware，TF的代码去哪里找，看下面
首先，打开网址wiki.ros.org，可以点击简体中文
然后，找到核心代码库即点击Core Libraries（核心库）
然后，找到Transforms/Coordinates，点击右边的tf 去看一下它的接口，上一节使用了静态广播也就是里面的6.3节

这一次是使用编程，点击第4节Code API Overview下的[Broadcasting Transforms]，找一下TF的API，可以看到使用的教程，我们点击下方的[c++]，竟然弃用了tf，没关系，我们参考tf2也一样，点击[Writing a tf2 broadcaster (C++)]
至此，我们教程搜寻完毕，但是，几乎没有任何作用，因为我还看不太懂，这里直接放代码：

这里新建ROS包，tf_transform
然后创建
发布节点tf_transform_pub.cpp：

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <tf/transform_broadcaster.h>
#include <sstream>

int main(int argc, char *argv[])
{

  ros::init(argc, argv, "tf_transform_pub");
  ros::NodeHandle n;

  tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(0.1, 0, 0.2));
  tf::Quaternion q;
  q.setRPY(0, 0, 3.141593);
  transform.setRotation(q);

  ros::Rate loop_rate(10);
  //每隔100ms发送一次

  int count = 0;
  while (ros::ok())
  {
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/base_link", "/laser"));
    //父机是base_link
    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }

  return 0;

}

订阅节点tf_transform_sub.cpp：

#include "ros/ros.h"
#include "std_msgs/String.h"

void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char *argv[])
{
  ros::init(argc, argv, "tf_transform_sub");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
  ros::spin();

  return 0;
}

至此，代码结束，但是使用debug编译仍然会出错，这是因为我们用到了tf的功能包，就需要在cmake放进去，既然使用了编译软件，我们就可以这样配置：
首先，打开tf_transform包下的CMakeLists.txt文件
然后，在如下代码出做修改：

##     * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
find_package(catkin REQUIRED COMPONENTS
  tf
  roscpp
)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed

加上tf即可
然后再编译，就可以成功了，