（ROS-4）通信demo

1 基于 topic 通信

1.1 自定义message（数据类型）

Message的定义类似C语言中的结构体。Message不仅仅是我们平时理解的一条一条的消息，而且更是ROS中topic的格式规范。或者可以理解msg是一个“类”，那么我们每次发布的内容可以理解为“对象”，这么对比来理解可能更加容易。
我们实际通常不会把Message概念分的那么清，通常说Message既指的是类，也是指它的对象。而msg文件则相当于类的定义了。

rosmsg相关指令：

rosmsg命令	作用
rosmsg list	列出系统上所有的msg
rosmsg show msg_name	显示某个msg的内容

1.1.1 新建功能包

首先进入src目录下创建功能包，

cd catkin_ws/src/
catkin_create_pkg pkg_a rospy roscpp std_msgs geometry_msgs message_generation message_runtime

依赖：rospy roscpp，是函数接口; std_msgs geometry_msgs，是消息嵌套用到的功能包;

message_generation message_runtime，是编译和运行所需的依赖，在这里添加，则可以省去后面修改package.xml的步骤。

1.1.2 新建msg文件

Message的定义位于功能包下的msg目录中（service则位于srv目录中）。

jw@G5-5500:~/catkin_ws$ roscd pkg_a/
jw@G5-5500:~/catkin_ws/src/pkg_a$ ls
CMakeLists.txt  include  package.xml  src
jw@G5-5500:~/catkin_ws/src/pkg_a$ mkdir msg && cd msg
jw@G5-5500:~/catkin_ws/src/pkg_a/msg$ gedit my_message.msg

msg的定义，类似结构体。

自定义的my_message.msg文件的内容如下:

• 顶行不能为空
• 中间用空格隔开而不是tab

geometry_msgs/Vector3 v2
geometry_msgs/Vector3 v1
string name
int32 num

geometry_msgs是ros的封装好的类型库，如对几何数据类型中的加速度，geometry_msgs/Accel，其msg内容为

jw@G5-5500:/opt/ros/melodic/share/geometry_msgs/msg$ cat Accel.msg 
# This expresses acceleration in free space broken into its linear and angular parts.
Vector3  linear
Vector3  angular

再看Vector3的消息定义

jw@G5-5500:/opt/ros/melodic/share/geometry_msgs/msg$ cat Vector3.msg 
# This represents a vector in free space. 
# It is only meant to represent a direction. Therefore, it does not
# make sense to apply a translation to it (e.g., when applying a 
# generic rigid transformation to a Vector3, tf2 will only apply the
# rotation). If you want your data to be translatable too, use the
# geometry_msgs/Point message instead.

float64 x
float64 y

1.1.3 修改package.xml与CMakeLists.txt

我们要确保msg文件被转换成为C++，Python和其他语言的源代码：
查看package.xml, 确保它包含一下两条语句:
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
如果没有，添加进去。 注意，在构建的时候，我们只需要"message_generation"。然而，在运行的时候，我们只需要"message_runtime"。

经过之前的catkin_create_pkg，已经自动把那两条语句添加进了package.xml，检查一下：

jw@G5-5500:~/catkin_ws/src/pkg_a$ ls
CMakeLists.txt  include  msg  package.xml  src
jw@G5-5500:~/catkin_ws/src/pkg_a$ gedit package.xml 

再修改CMakeLists.txt，包括4个点：

jw@G5-5500:~/catkin_ws/src/pkg_a$ gedit CMakeLists.txt

• 1,find_package，删除message_runtime

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  message_generation
  # message_runtime ，删除或注释掉
  roscpp
  rospy
  std_msgs
)

• 2,catkin_package，取消注释并修改

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES pkg_a
CATKIN_DEPENDS geometry_msgs  message_runtime roscpp rospy std_msgs # 去掉了message_generation
#  DEPENDS system_lib
)

• 3,add_message_files;取消注释并修改

 add_message_files(
   FILES
   my_message.msg
 )

• 4,generate_messages;取消注释并修改，添加依赖的msg包

generate_messages(
  DEPENDENCIES
  std_msgs
  geometry_msgs
)

1.1.4 编译

然后进入workplace编译一下，

cd catkin_ws/
catkin_make

编译成功，并且也能查找得到：

jw@G5-5500:~/catkin_ws$ catkin_find_pkg pkg_a
src/pkg_a
jw@G5-5500:~/catkin_ws$ rosmsg list | grep my
pkg_a/my_message
jw@G5-5500:~/catkin_ws$ rosmsg show my_message
[pkg_a/my_message]:
geometry_msgs/Vector3 v2
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 v1
  float64 x
  float64 y
  float64 z
string name
int32 num

1.2 cpp

1.2.1 订阅与发布

roscd pkg_a/src/
gedit puber.cpp 
gedit suber.cpp

wiki讲解链接

发布的步骤：

• 创建发布者：advertise
• 发布消息，或对发布者句柄进行操作

//puber.cpp
#include "ros/ros.h"
#include "pkg_a/my_message.h"
#include "sstream"

int main(int argc, char **argv)
{
  std::stringstream ss;
  pkg_a::my_message msg;
  ros::init(argc, argv, "puber");
  ros::NodeHandle h;
  ROS_INFO("Im %s", ros::this_node::getName().c_str());
  ros::Publisher puber = h.advertise<pkg_a::my_message>("my_topic", 10);
  ros::Rate loop_rate(1);
  ss << "A B";

  while (ros::ok())
  {
    
    ss >> msg.name;
    ss >> msg.num;
    ROS_INFO("I Send [name:%s, num:%d]", msg.name.c_str(), msg.num);
    puber.publish(msg);
    loop_rate.sleep();
  }
  return 0;
}

订阅的步骤：

• 创建订阅者，传入回调函数
• 阻塞等待消息到来

//suber.cpp
#include "ros/ros.h"
#include "pkg_a/my_message.h"

void infoCallback(const pkg_a::my_message::ConstPtr& msg)
// 回调函数名(常量 包名::消息::常量指针 变量名), 用于接收消息，且防止被修改
{
  ROS_INFO("Name Changed [%s %d]", msg->name.c_str(), msg->num);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "suber");
  ros::NodeHandle n;
  ROS_INFO("Im %s", ros::this_node::getName().c_str());
  ros::Subscriber sub = n.subscribe("my_topic", 1000, infoCallback);
  // 实例化订阅器，且订阅"my_topic"这个话题，并调用回调函数infoCallback
  ros::spin();
  // 有回调函数记得加上这句话
  return 0;
}

1.2.2 修改CmakeLists.txt

除了创建消息的时候，我们修改了功能包pkg_a的CMakeList.xt关于话题使用的消息的配置，现在是节点配置，${PROJECT_NAME}_node直接换成节点名称，其他按照模板填写：

# add_executable(${PROJECT_NAME}_node src/pkg_a_node.cpp)
add_executable(puber src/puber.cpp)
add_executable(suber src/suber.cpp)
# target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} )
target_link_libraries(puber ${catkin_LIBRARIES})
target_link_libraries(suber ${catkin_LIBRARIES})
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

1.2.3 编译运行

cd ~/catkin_ws
catkin_make

• Master

roscore

• suber

jw@G5-5500:~/catkin_ws$ rosrun pkg_a suber 
[ INFO] [1618652950.893544133]: Im /suber
[ INFO] [1618652951.900597800]: Name Changed [A 0]

• puber

jw@G5-5500:~/catkin_ws$ rosrun pkg_a puber
[ INFO] [1618652937.898728459]: Im /puber
[ INFO] [1618652937.899726548]: I Send [name:A, num:0]

1.3 python

1.3.1 puber.py

# !/usr/bin/env python
# -*- coding: <encoding name> -*- : # -*- coding: utf-8 -*-
import rospy
from pkg_a.msg import my_message  # 从pkg_a的msg目录下导入my_message.msg
def talker():
	pub = rospy.Publisher('my_topic', my_message, queue_size=10)  # 实例化一个发布者
	rospy.init_node('pyPuber', anonymous=True)  # 初始化节点信息
	rospy.loginfo("Im %s"%rospy.get_name()) # 等同ROS_INFO
	rate = rospy.Rate(10)
	a = my_message()  # 实例化消息
	while not rospy.is_shutdown():
		a.name = "PY"
		a.num = 0
		rospy.loginfo("I sended :%s %d"%(a.name, a.num)) # 等同ROS_INFO
		pub.publish(a)
		rate.sleep()

if __name__ == '__main__':
	try:
		talker()
	except rospy.ROSInterruptException:
		pass

1.3.2 suber.py

#!/usr/bin/env python
# -*- coding: <encoding name> -*- : # -*- coding: utf-8 -*-
import rospy
from pkg_a.msg import my_message  # 从pkg_a的msg目录下导入my_message.msg

def callback(data):
	rospy.loginfo("I receieved :%s %d"%(data.name, data.num))
def listener():
	rospy.init_node('pySuber', anonymous=True)
	rospy.loginfo("Im %s"%rospy.get_name()) # 等同ROS_INFO
	rospy.Subscriber("my_topic", my_message, callback)
	rospy.spin()
if __name__ == '__main__':
    listener()

1.3.3 编译运行

在1.1的基础上，使用自定义的message;

python不需要编译，只需要将.py文件修改为可执行文件即可：

chmod +x puber.py suber.py

运行：

roscore
rosrun pkg_a suber.py 
rosrun pkg_a puber.py 

2 基于 service 通信

2.1 自定义service（包含了request与reply）

rossrv相关指令：

$ rossrv
rossrv is a command-line tool for displaying information about ROS Service types.

Commands:
        rossrv show     Show service description
        rossrv list     List all services
        rossrv md5      Display service md5sum
        rossrv package  List services in a package
        rossrv packages List packages that contain services

Type rossrv <command> -h for more detailed usage

2.1.1 新建功能包

jw@G5-5500:~/catkin_ws/src$ catkin_create_pkg pkg_b std_msgs roscpp rospy message_generation geometry_msgs

自定义service与自定义message相比较，依赖少了message_runtime；但是都存在依赖message_generation。同样，创建时加入依赖，可以省去修改package.xml的步骤。

2.1.2 新建srv文件

进入pkg_b的目录下，新建srv文件

jw@G5-5500:~/catkin_ws/src/pkg_b$ mkdir srv && cd srv
jw@G5-5500:~/catkin_ws/src/pkg_b/srv$ gedit add.srv

add.srv的内容如下：

int64 a
int64 b
geometry_msgs/Vector3 v1
---
int64 sum
geometry_msgs/Vector3 v2

2.1.3 修改CMakeList.txt与package.xml文件

其实，在一开始创建包时，传入了依赖message_generation，因此package.xml文件已经添加好了，不需要我们再去修改。

修改CMakeList.txt的两处地方

generate_messages(
  DEPENDENCIES
  geometry_msgs
  std_msgs
)
add_service_files(
  FILES
  add.srv
)

2.1.4 回到工作空间下编译

cd ~/catkin_ws
catkin_make

jw@G5-5500:~/catkin_ws$ rossrv list | grep add
pkg_b/add

jw@G5-5500:~/catkin_ws$ rossrv info add
[pkg_b/add]:
int64 a
int64 b
geometry_msgs/Vector3 v1
  float64 x
  float64 y
  float64 z
---
int64 sum
geometry_msgs/Vector3 v2
  float64 x
  float64 y
  float64 z

2.2 cpp

2.2.1 请求与应答

在pkg_b/src/目录下创建2个cpp，一个作为client，一个作为server。

cd ~/catkin_ws/src/pkg_b/src
gedit client.cpp
gedit server.cpp

请求的步骤：

• 创建一个客户端
• 客户端调用call函数，进行请求，并阻塞等待应答

// client.cpp
#include "ros/ros.h"
#include "pkg_b/add.h"
#include <cstdlib>

int main(int argc, char **argv)
{
	ros::init(argc, argv, "client");
	ros::NodeHandle n;
	// 创建一个client，"add"是请求服务的名称（在server端会创建一个"add"的服务）
	ros::ServiceClient client = n.serviceClient<pkg_b::add>("add");
	server::add srv;				// 创建变量作为数据容器，实质是一个结构体
	ros::Rate loop_rate(1);
	long int a,b;
    while(ros::ok)
    {
        a = rand()%100;
        b = rand()%100;
        srv.request.a = a;			// 结构体内有request与response
        srv.request.b = b;
        ROS_INFO("Send a request: %ld + %ld = ?", a, b);
        if (client.call(srv))		// 想服务端请求服务，并返回成功与否的bool值
        {
            ROS_INFO("Sum: %ld", (long int)srv.response.sum);
        }
        else
        {
            ROS_ERROR("Failed to call service add");
            return 1;
        }
        loop_rate.sleep();
    }
    return 0;
}

响应的步骤：

• 创建一个服务器，传入回调函数
• 阻塞等待，收到请求则启动回调函数进行处理。

// server.cpp
#include "ros/ros.h"
#include "pkg_b/add.h"

// 回调函数需要传入Request与Response
bool add(pkg_b::add::Request  &req, pkg_b::add::Response &res)
{
	res.sum = req.a + req.b;
    ROS_INFO("Get request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
    return true;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "server");
    ros::NodeHandle n;
    ROS_INFO("The server is start running!");
    ros::ServiceServer service = n.advertiseService("add", add);
    ros::spin();

    return 0;
}

2.2.2 修改CMakeList.txt

在pkg_b目录下的CMakeList.txt中添加：

add_executable(ser src/server.cpp)
add_executable(cli src/client.cpp)

target_link_libraries(ser ${catkin_LIBRARIES})
target_link_libraries(cli ${catkin_LIBRARIES})

注意，这里的ser与cli将会成为c++编译后的可执行文件的名称。

2.2.2 编译运行

cd ~/catkin_ws
catkin_make

2.3 python

在2.1.4的基础上，增加2个py文件即可

2.3.1 订阅与发布

• client.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import sys
import rospy
import random
from pkg_b.srv import add, addResponse, addRequest
from geometry_msgs.msg import Vector3
def sendrequest(x, y):
	rospy.wait_for_service('add')
	# 等待服务端启动，会阻塞在这里，直到有add这个服务出现
	try:
		req = rospy.ServiceProxy('add', add)  # 实例化一个请求
		send_data = addRequest(x, y, Vector3())
		resp1 = req(send_data)  # 进行请求并返回
		return resp1.sum
	except rospy.ServiceException, e:
		print "Service call failed: %s"%e
		return 0
def runforever():
	rospy.init_node('add_client', anonymous=True)
	rate = rospy.Rate(1)  # 控制一秒一个请求
	while not rospy.is_shutdown():
		x = int(random.random()*100)
		y = int(random.random()*100)
		print "Requesting %d+%d"%(x, y)
		print "%d + %d = %d"%(x, y, sendrequest(x, y))
		rate.sleep()
	
if __name__ == "__main__":
	runforever()

• server.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
from pkg_b.srv import add, addResponse
import rospy

def handle_add_two_ints(req):# 服务的回调函数
	print("Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b)))
	return addResponse(req.a + req.b, req.v1)

def add_two_ints_server():
	rospy.init_node('add_server')
	s = rospy.Service('add', add, handle_add_two_ints)
	# 实例化服务端，并设置服务名，数据类型与回调函数
	print ("Ready to add two ints.")
	rospy.spin()  # 进入死循环只处理回调

if __name__ == "__main__":
	add_two_ints_server()

2.3.2 编译运行

编辑好client与server2个py文件后，还没有赋予它们执行权限：

jw@G5-5500:~/catkin_ws/src/pkg_b/scripts$ ls -l
总用量 8
-rw-rw-r-- 1 jw jw 894 4月  18 19:41 client.py
-rw-rw-r-- 1 jw jw 565 4月  18 19:41 server.py

（1）赋予它们执行权限的：chmod

jw@G5-5500:~/catkin_ws/src/pkg_b/scripts$ chmod +x client.py server.py
jw@G5-5500:~/catkin_ws/src/pkg_b/scripts$ ls -l
总用量 8
-rwxrwxr-x 1 jw jw 894 4月  18 19:41 client.py
-rwxrwxr-x 1 jw jw 565 4月  18 19:41 server.py

可以看到，增加了执行权限x

（2）将以下内容添加到CMakeLists.txt中，这样可以确保正确安装了python脚本，并使用了正确的python解释器。

• 在CMakeLists.txt中取消注释并修改catkin_install_python：

catkin_install_python(PROGRAMS
  scripts/server.py
  scripts/client.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

• 修改了CMakeLists.txt后需要在catkin_ws/目录下编译

# 全编
catkin_make
# 最好还是使用单编
catkin_make -DCATKIN_WHITELIST_PACKAGES=" your package name"

（3）运行

roscore
rosrun pkg_b server.py
rosrun pkg_b client.py

3 总结cpp与python的区别

3.1 生成msg与srv的步骤没有cpp与python的区分

3.1.1 自定义msg

• 在package的msg目录下创建.msg文件

• 修改package.xml

  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>

• 修改CMakeLists.txt

  find_package()

  catkin_package()

  add_message_files()

  generate_messages()

• 进入workplace编译生产.h文件，catkin_Make

3.1.2 自定义srv

• 在package的srv目录下创建.srv文件

• 修改package.xml

  <build_depend>message_generation</build_depend> <exec_depend>message_runtime</exec_depend>

• 修改CMakeLists.txt

  find_package()

  add_service_files()

  generate_messages()

• 进入workplace编译生产.h文件，catkin_Make

3.2 生成cpp文件

• 在package的src/目录下创建cpp文件

• 修改CMakeLists.txt

  必须添加，才能生产可执行文件：

  add_executable()

  target_link_libraries()

  视情况修改文件依赖：

  find_package()

  catkin_package()

• 编译，最好是使用单独编译

catkin_make -DCATKIN_WHITELIST_PACKAGES=" your package name"

3.3 生成py文件

• 在package的scripts/目录下创建py文件
• chmod +x 增加py文件的可执行权限
• 无需编译直接可以运行

3.4 cpp与py的区别总结

3.4.1 编译

Python是一种解释语言，而C++是一种编译语言。就如同我们存放的位置所说，.py文件是放在/script文件下，就说明了其是动态的，不需要编译，我们修改了.py的内容后，可以直接运行；C++则是调用其编译后的产物，也就是说，我们运行的时候，根本不需要它的.cpp这样的源码，所以我们在修改了.cpp后，需要重新编译。

3.4.2 依赖

我们创建的消息、服务是需要通过庞大的make系统调用一系列东西来生成，所以在两者Python与C++编写CMakeList.txt生成消息文件，服务文件的时候，这些代码基本上是一样的，所以都需要编写、修改：find_package、add_message_files、add_service_files、generate_messages、catkin_package等之类的内容。

• C++为了生成可执行文件，我们还得添加add_executable、target_link_libraries这样的内容。

• python需对py文件添加可执行权限：chmod -x filename.py

3.4.3 程序的编写

（1）头文件/包名

• C++头文件的位置在/devel/include/功能包名/里
• Python的生成的文件在/devel/lib/python2.7/dist-packages/功能包名/里

（2）spin与spinOnce

在C++里，提供了spin与spinOnce。两者的区别就是：

• spin运行后，直接阻塞在这里，一直检查回调函数状态，不会执行后面的代码，直到节点销毁。
• spinOnce是检查一下回调函数状态，若回调了，就先处理，处理完后继续执行后面的代码，需要配合ros::ok()来进行循环操作。

在Python里，却只提供了spin函数，那么我们不想让主线程阻塞的话，只有为我们的spin函数整一个线程：

import threading
def goaway():
    rospy.spin()


if __name__ == "__main__":
    spin_thread = threading.Thread(target = goaway)
    spin_stread.start()