ROS-回炉重铸版

参考资料：ROS文档http://www.autolabor.com.cn/book/ROSTutorials/

2 ROS通信机制

2.1 话题通信

2.2 服务通信

2.2.1 服务通信理论模型

2.2.2 服务通信自定义srv调用

（1）客户端代码

#! /usr/bin/env python3

import rospy
from plumbing_server_client.srv import AddInts,AddIntsRequest,AddIntsResponse
'''
    服务端：解析客户端请求，产生响应。
        1.导包
        2.初始化ROS节点
        3.创建服务端对象
        4.处理逻辑(回调函数)
        5.spin()
'''
# 参数：封装请求数据
# 返回值：相应数据
def doNum(request):
    # 1.解析提交的两个整数
    num1 = request.num1
    num2 = request.num2
    # 2.求和
    sum = num1 + num2
    # 3.将结果封装进相应
    response = AddIntsResponse()
    response.sum = sum

    rospy.loginfo("服务器解析的数据num1 = %d,num2 = %d, sum = %d", num1, num2, sum)

    return response


if __name__ == "__main__":
    # 2.初始化ROS节点
    rospy.init_node("heiShui")
    # 3.创建服务端对象
    server = rospy.Service("addInts",AddInts,doNum)
    rospy.loginfo('服务器已经启动')
    # 4.处理逻辑(回调函数)
    # 5.spin()
    rospy.spin()

（2）服务端代码 

#! /usr/bin/env python3

import rospy
from plumbing_server_client.srv import AddInts,AddIntsRequest,AddIntsResponse

'''
    客户端：组织并提交请求，处理服务端响应。
        1.导包
        2.初始化ROS节点
        3.创建客户端对象
        4.组织请求数据，并发送请求
        5.处理响应
'''

if __name__ == "__main__":
    # 2.初始化ROS节点
    rospy.init_node("erHei")
    # 3.创建客户端对象
    client = rospy.ServiceProxy("addInts",AddInts)
    # 4.组织请求数据，并发送请求
    response = client.call(12,34)
    # 5.处理响应
    rospy.loginfo("响应的数据：%d",response.sum)

 服务端代码优化

#! /usr/bin/env python3

import rospy
from plumbing_server_client.srv import AddInts,AddIntsRequest,AddIntsResponse
import sys
'''
    客户端：组织并提交请求，处理服务端响应。
        1.导包
        2.初始化ROS节点
        3.创建客户端对象
        4.组织请求数据，并发送请求
        5.处理响应

        优化实现：
            可以在执行节点时，动态传入参数
'''

if __name__ == "__main__":

    # 判断参数长度
    if len(sys.argv) != 3: # 数组‘零’的位置是文件名
        rospy.logerr("传入的参数个数不对")
        sys.exit(1)

    # 2.初始化ROS节点
    rospy.init_node("erHei")
    # 3.创建客户端对象
    client = rospy.ServiceProxy("addInts",AddInts)
    # 4.组织请求数据，并发送请求
    # 解析传入的参数
    num1 = int(sys.argv[1])
    num2 = int(sys.argv[2])
    response = client.call(num1,num2)
    # 5.处理响应
    rospy.loginfo("响应的数据：%d",response.sum)

服务端代码优化 

#! /usr/bin/env python3

import rospy
from plumbing_server_client.srv import AddInts,AddIntsRequest,AddIntsResponse
import sys
'''
    客户端：组织并提交请求，处理服务端响应。
        1.导包
        2.初始化ROS节点
        3.创建客户端对象
        4.组织请求数据，并发送请求
        5.处理响应

        优化实现：
            可以在执行节点时，动态传入参数
        
        问题：
            客户端先于服务端启动，会抛出异常
        需要：
            客户端先于服务端启动之后，不要抛出异常而是挂起，等待服务启动后，再次发送请求
        实现：
            ROS中内置了相关函数，这些函数可以判断服务器的状态，如果服务没有启动，那么就让客户端挂起

            方案1：
            client.wait_for_service()
            方案2：
            rospy.wait_for_service("addInts") 需要一个参数：话题名称
'''

if __name__ == "__main__":

    # 判断参数长度
    if len(sys.argv) != 3: # 数组‘零’的位置是文件名
        rospy.logerr("传入的参数个数不对")
        sys.exit(1)

    # 2.初始化ROS节点
    rospy.init_node("erHei")
    # 3.创建客户端对象
    client = rospy.ServiceProxy("addInts",AddInts)
    # 4.组织请求数据，并发送请求
    # 解析传入的参数
    num1 = int(sys.argv[1])
    num2 = int(sys.argv[2])
    # 等待服务启动
    # client.wait_for_service()
    rospy.wait_for_service("addInts")
    response = client.call(num1,num2)
    # 5.处理响应
    rospy.loginfo("响应的数据：%d",response.sum)

2.3 参数服务器

2.3.1 参数服务器理论模型

2.3.2 参数操作

 （1）参数服务器新增（修改）参数

#! /usr/bin/env python

import rospy
"""
    演示参数的新增与修改
    需求：在参数服务器中设置机器人属性，型号，半径
    实现：
        rospy.set_param()
"""
if __name__ == "__main__":
    # 初始化ROS节点
    rospy.init_node("param_set_p")
    # 新增参数
    rospy.set_param("type_p","xiaoHuangChe")
    rospy.set_param("radius_p",0.15)
    # 修改参数
    rospy.set_param("radius_p",0.20)
    pass

（2）参数服务器获取参数

#! /usr/bin/env python3

import rospy
"""
    演示参数查询：
        get_param(键,默认值)
            当键存在时，返回对应的值，如果不存在返回默认值
        get_param_cached
            和get_param使用一致，只是效率高
        get_param_names
            获取所有的参数的键的集合
        has_param
            判断是否包含某个键
        search_param
            查找某个参数的键，并返回完整的键名
"""
if __name__ == "__main__":
    # 初始化ROS节点
    rospy.init_node("get_param_p")
    # 1.get_param
    radius = rospy.get_param("radius_p",0.5)
    rospy.loginfo("radius = %.2f", radius)
    radius2 = rospy.get_param("radius_p_xxx",0.5)
    rospy.loginfo("radius2 = %.2f", radius2)

    # 2.get_param_cached
    radius3 = rospy.get_param_cached("radius_p",0.5)
    rospy.loginfo("radius3 = %.2f", radius3)
    radius4 = rospy.get_param_cached("radius_p_xxx",0.5)
    rospy.loginfo("radius4 = %.2f", radius4)

    # 3.get_param_names
    names = rospy.get_param_names()
    for name in names:
        rospy.loginfo("name = %s", name)

    # 4.has_param
    flag1 = rospy.has_param("radius_p")
    if flag1:
        rospy.loginfo("radius_p存在")
    else:
        rospy.loginfo("radius_p不存在")

    flag2 = rospy.has_param("radius_p_xxx")
    if flag2:
        rospy.loginfo("radius_p_xxx存在")
    else:
        rospy.loginfo("radius_p_xxx不存在")

    # 5.search_param
    key = rospy.search_param("radius_p")
    rospy.loginfo("key = %s",key)

（3）参数服务器删除参数

#! /usr/bin/env python3

import rospy
"""
    演示参数的删除：
        delete_param()
"""

if __name__ == "__main__":
    rospy.init_node("del_param_p")

    try:
    # 删除参数
        rospy.delete_param("radius_p")
    except Exception as e:
        rospy.loginfo("被删除的参数不存在")
        

2.4 常用命令

• rosnode : 操作节点
• rostopic : 操作话题
• rosservice : 操作服务
• rosmsg : 操作msg消息
• rossrv : 操作srv消息
• rosparam : 操作参数

2.5 通信机制实操

2.5.1 话题发布

#! /usr/bin/env python3

import rospy
from geometry_msgs.msg import Twist
"""
    发布方：发布速度消息
        话题：/turtle1/cmd_vel
        消息类型：geometry_msgs/Twist

    1.导包
    2.初始化ROS节点
    3.创建发布者对象
    4.组织数据并发布数据

"""

if __name__ == '__main__':
    # 2.初始化ROS节点
    rospy.init_node("my_control_p")
    # 3.创建发布者对象
    pub = rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size=10)
    # 4.组织数据并发布数据
    # 设置发布频率
    rate = rospy.Rate(10)
    # 创建速度消息
    twist = Twist()
    twist.linear.x = 0.5
    twist.linear.y = 0.0
    twist.linear.z = 0.0 

    twist.angular.x = 0.0
    twist.angular.y = 0.0
    twist.angular.z = 0.5   
    # 循环发布
    while not rospy.is_shutdown():
        pub.publish(twist)
        rate.sleep()