机器人运动学仿真软件：ROS (Robot Operating System)_（4）.ROS消息与服务

最新推荐文章于 2025-05-20 06:14:02 发布

kkchenjj

最新推荐文章于 2025-05-20 06:14:02 发布

阅读量610

点赞数 17

分类专栏：机器人仿真文章标签：机器人机器人仿真开发语言性能优化

本文链接：https://blog.csdn.net/weixin_42749425/article/details/148053986

版权

机器人仿真专栏收录该内容

79 篇文章

订阅专栏

ROS消息与服务

在ROS中，消息（Messages）和服务（Services）是两个核心概念，用于实现节点之间的通信。本节将详细介绍如何使用消息和服务进行机器人运动学仿真软件的二次开发，包括它们的定义、使用方法以及具体的代码示例。

在这里插入图片描述

消息（Messages）

消息的概念

消息是ROS中用于在节点之间传递数据的载体。每个消息都有一个特定的类型，这个类型定义了消息的数据结构。消息类型通常存储在.msg文件中，这些文件是ROS包的一部分。常见的消息类型包括std_msgs、sensor_msgs、geometry_msgs等。

消息的定义

消息类型可以通过创建.msg文件来定义。例如，假设我们需要定义一个包含机器人关节角度的数据消息，可以创建一个名为JointAngles.msg的文件，内容如下：


# JointAngles.msg

float64 shoulder

float64 elbow

float64 wrist

消息的发布与订阅

在ROS中，发布消息的节点称为发布者（Publisher），订阅消息的节点称为订阅者（Subscriber）。发布者将消息发送到特定的主题（Topic），订阅者则从该主题接收消息。

发布者示例

以下是一个使用Python编写的发布者节点示例，该节点发布机器人关节角度的消息：


#!/usr/bin/env python

import rospy

from std_msgs.msg import Float64

from my_robot_msgs.msg import JointAngles



def talker():

    # 初始化节点

    rospy.init_node('joint_angles_publisher', anonymous=True)

    

    # 创建一个发布者，发布到名为'joint_angles'的主题

    pub = rospy.Publisher('joint_angles', JointAngles, queue_size=10)

    

    # 设置发布频率

    rate = rospy.Rate(1)  # 1 Hz

    

    while not rospy.is_shutdown():

        # 创建消息实例

        joint_angles_msg = JointAngles()

        joint_angles_msg.shoulder = 0.5

        joint_angles_msg.elbow = 1.0

        joint_angles_msg.wrist = 1.5

        

        # 发布消息

        pub.publish(joint_angles_msg)

        

        # 打印日志

        rospy.loginfo("Published joint angles: shoulder=%f, elbow=%f, wrist=%f", 

                     joint_angles_msg.shoulder, joint_angles_msg.elbow, joint_angles_msg.wrist)

        

        # 等待下一次发布

        rate.sleep()



if __name__ == '__main__':

    try:

        talker()

    except rospy.ROSInterruptException:

        pass

订阅者示例

以下是一个使用Python编写的订阅者节点示例，该节点订阅机器人关节角度的消息：


#!/usr/bin/env python

import rospy

from my_robot_msgs.msg import JointAngles



def callback(joint_angles_msg):

    # 处理接收到的消息

    rospy.loginfo("Received joint angles: shoulder=%f, elbow=%f, wrist=%f", 

                 joint_angles_msg.shoulder, joint_angles_msg.elbow, joint_angles_msg.wrist)



def listener():

    # 初始化节点

    rospy.init_node('joint_angles_subscriber', anonymous=True)

    

    # 创建一个订阅者，订阅名为'joint_angles'的主题

    rospy.Subscriber('joint_angles', JointAngles, callback)

    

    # 保持节点运行

    rospy.spin()



if __name__ == '__main__':

    listener()

消息的编译

在创建了.msg文件后，需要编译这些文件以生成对应的Python或C++代码。编译消息文件可以通过以下步骤完成：

在包的CMakeLists.txt文件中添加以下内容：


find_package(catkin REQUIRED COMPONENTS

  rospy

  std_msgs

  message_generation

)



add_message_files(

  FILES

  JointAngles.msg

)



generate_messages(

  DEPENDENCIES

  std_msgs

)



catkin_package(

  CATKIN_DEPENDS message_runtime

)

在包的package.xml文件中添加以下内容：


<build_depend>message_generation</build_depend>

<exec_depend>message_runtime</exec_depend>

编译包：


cd ~/catkin_ws

catkin_make

source devel/setup.bash

消息的数据类型

ROS支持多种数据类型，包括基本数据类型（如int32、float64）、数组类型（如int32[]）、字符串类型（string）和自定义消息类型。自定义消息类型可以包含其他消息类型，形成复杂的数据结构。

消息的可视化

消息可以通过rqt工具进行可视化。例如，可以使用rqt_plot来实时显示关节角度的变化：


rosrun rqt_plot rqt_plot

在rqt_plot中，选择/joint_angles/shoulder、/joint_angles/elbow和/joint_angles/wrist主题，即可看到关节角度的实时变化曲线。

服务（Services）

服务的概念

服务是ROS中用于实现请求-响应模式通信的机制。服务提供者（Server）等待请求，一旦收到请求，它会处理请求并返回响应。服务请求者（Client）发送请求并等待响应。

服务的定义

服务类型通过创建.srv文件来定义。例如，假设我们定义一个服务，用于设置机器人的关节角度，可以创建一个名为SetJointAngles.srv的文件，内容如下：


# SetJointAngles.srv

float64 shoulder

float64 elbow

float64 wrist

---

bool success

string message

服务的服务器示例

以下是一个使用Python编写的服务器节点示例，该节点提供设置机器人关节角度的服务：


#!/usr/bin/env python

import rospy

from my_robot_msgs.srv import SetJointAngles, SetJointAnglesResponse



def handle_set_joint_angles(req):

    # 处理请求

    rospy.loginfo("Setting joint angles: shoulder=%f, elbow=%f, wrist=%f", 

                 req.shoulder, req.elbow, req.wrist)

    

    # 假设设置关节角度成功

    success = True

    message = "Joint angles set successfully"

    

    # 返回响应

    return SetJointAnglesResponse(success, message)



def set_joint_angles_server():

    # 初始化节点

    rospy.init_node('set_joint_angles_server')

    

    # 创建一个服务，名为'set_joint_angles'

    s = rospy.Service('set_joint_angles', SetJointAngles, handle_set_joint_angles)

    

    # 保持节点运行

    rospy.spin()



if __name__ == '__main__':

    set_joint_angles_server()

服务的客户端示例

以下是一个使用Python编写的客户端节点示例，该节点请求设置机器人关节角度的服务：


#!/usr/bin/env python

import rospy

from my_robot_msgs.srv import SetJointAngles, SetJointAnglesRequest



def set_joint_angles_client():

    # 初始化节点

    rospy.init_node('set_joint_angles_client')

    

    # 等待服务可用

    rospy.wait_for_service('set_joint_angles')

    

    try:

        # 创建服务代理

        set_joint_angles = rospy.ServiceProxy('set_joint_angles', SetJointAngles)

        

        # 创建请求

        req = SetJointAnglesRequest()

        req.shoulder = 0.7

        req.elbow = 1.2

        req.wrist = 1.8

        

        # 发送请求并接收响应

        res = set_joint_angles(req)

        

        # 处理响应

        if res.success:

            rospy.loginfo("Service call succeeded: %s", res.message)

        else:

            rospy.logwarn("Service call failed: %s", res.message)

    except rospy.ServiceException as e:

        rospy.logerr("Service call failed: %s", e)



if __name__ == '__main__':

    set_joint_angles_client()

服务的编译

在创建了.srv文件后，需要编译这些文件以生成对应的Python或C++代码。编译服务文件的步骤与编译消息文件类似：

在包的CMakeLists.txt文件中添加以下内容：


find_package(catkin REQUIRED COMPONENTS

  rospy

  std_msgs

  message_generation

)



add_service_files(

  FILES

  SetJointAngles.srv

)



generate_messages(

  DEPENDENCIES

  std_msgs

)



catkin_package(

  CATKIN_DEPENDS message_runtime

)

在包的package.xml文件中添加以下内容：


<build_depend>message_generation</build_depend>

<exec_depend>message_runtime</exec_depend>

编译包：


cd ~/catkin_ws

catkin_make

source devel/setup.bash

服务的调用

服务可以通过ROS命令行工具rosservice进行调用。例如，调用set_joint_angles服务：


rosservice call /set_joint_angles "shoulder: 0.7

elbow: 1.2

wrist: 1.8"

服务的可视化

服务的请求和响应可以通过rqt_service_client工具进行可视化。例如，使用rqt_service_client调用set_joint_angles服务：


rosrun rqt_service_client rqt_service_client

在rqt_service_client中，选择/set_joint_angles服务，输入请求参数，即可看到服务的响应。

消息和服务的结合使用

在实际应用中，消息和服务通常结合使用，以实现更复杂的通信需求。例如，可以通过发布消息来控制机器人的运动，并通过服务来获取机器人的当前状态。

实例：控制机器人运动并获取状态

假设我们有一个机器人，可以通过发布JointAngles消息来控制其关节角度，并通过GetJointAngles服务来获取当前的关节角度。以下是一个完整的示例。

消息定义


# JointAngles.msg

float64 shoulder

float64 elbow

float64 wrist

服务定义


# GetJointAngles.srv

---

float64 shoulder

float64 elbow

float64 wrist

发布者节点


#!/usr/bin/env python

import rospy

from std_msgs.msg import Float64

from my_robot_msgs.msg import JointAngles



def talker():

    # 初始化节点

    rospy.init_node('joint_angles_publisher', anonymous=True)

    

    # 创建一个发布者，发布到名为'joint_angles'的主题

    pub = rospy.Publisher('joint_angles', JointAngles, queue_size=10)

    

    # 设置发布频率

    rate = rospy.Rate(1)  # 1 Hz

    

    while not rospy.is_shutdown():

        # 创建消息实例

        joint_angles_msg = JointAngles()

        joint_angles_msg.shoulder = 0.5

        joint_angles_msg.elbow = 1.0

        joint_angles_msg.wrist = 1.5

        

        # 发布消息

        pub.publish(joint_angles_msg)

        

        # 打印日志

        rospy.loginfo("Published joint angles: shoulder=%f, elbow=%f, wrist=%f", 

                     joint_angles_msg.shoulder, joint_angles_msg.elbow, joint_angles_msg.wrist)

        

        # 等待下一次发布

        rate.sleep()



if __name__ == '__main__':

    try:

        talker()

    except rospy.ROSInterruptException:

        pass

服务服务器节点


#!/usr/bin/env python

import rospy

from my_robot_msgs.srv import GetJointAngles, GetJointAnglesResponse



class JointAnglesServer:

    def __init__(self):

        # 初始化节点

        rospy.init_node('get_joint_angles_server')

        

        # 创建服务，名为'get_joint_angles'

        self.service = rospy.Service('get_joint_angles', GetJointAngles, self.handle_get_joint_angles)

        

        # 当前关节角度

        self.current_joint_angles = JointAngles()

        self.current_joint_angles.shoulder = 0.0

        self.current_joint_angles.elbow = 0.0

        self.current_joint_angles.wrist = 0.0

        

        # 订阅关节角度主题

        self.sub = rospy.Subscriber('joint_angles', JointAngles, self.update_joint_angles)

    

    def update_joint_angles(self, msg):

        # 更新当前关节角度

        self.current_joint_angles = msg

    

    def handle_get_joint_angles(self, req):

        # 处理获取关节角度的请求

        rospy.loginfo("Returning current joint angles: shoulder=%f, elbow=%f, wrist=%f", 

                     self.current_joint_angles.shoulder, self.current_joint_angles.elbow, self.current_joint_angles.wrist)

        

        # 返回响应

        return GetJointAnglesResponse(self.current_joint_angles.shoulder, 

                                     self.current_joint_angles.elbow, 

                                     self.current_joint_angles.wrist)



if __name__ == '__main__':

    server = JointAnglesServer()

    rospy.spin()

服务客户端节点


#!/usr/bin/env python

import rospy

from my_robot_msgs.srv import GetJointAngles, GetJointAnglesRequest



def get_joint_angles_client():

    # 初始化节点

    rospy.init_node('get_joint_angles_client')

    

    # 等待服务可用

    rospy.wait_for_service('get_joint_angles')

    

    try:

        # 创建服务代理

        get_joint_angles = rospy.ServiceProxy('get_joint_angles', GetJointAngles)

        

        # 创建请求

        req = GetJointAnglesRequest()

        

        # 发送请求并接收响应

        res = get_joint_angles(req)

        

        # 处理响应

        rospy.loginfo("Current joint angles: shoulder=%f, elbow=%f, wrist=%f", 

                     res.shoulder, res.elbow, res.wrist)

    except rospy.ServiceException as e:

        rospy.logerr("Service call failed: %s", e)



if __name__ == '__main__':

    get_joint_angles_client()

运行示例

启动ROS核心：


roscore

启动发布者节点：


rosrun my_robot_package joint_angles_publisher.py

启动服务服务器节点：


rosrun my_robot_package get_joint_angles_server.py

启动服务客户端节点：


rosrun my_robot_package get_joint_angles_client.py

通过以上步骤，发布者节点将不断发布机器人的关节角度，服务服务器节点将实时更新这些角度，并通过服务提供给客户端节点。

消息和服务的高级用法

动态重配置

ROS提供了dynamic_reconfigure工具，用于在运行时动态调整节点的参数。例如，可以动态调整关节角度的发布频率。

动态重配置的服务器端示例


#!/usr/bin/env python

import rospy

import dynamic_reconfigure.server

from my_robot_cfg.cfg import JointAnglesConfig

from std_msgs.msg import Float64

from my_robot_msgs.msg import JointAngles



class JointAnglesPublisher:

    def __init__(self):

        # 初始化节点

        rospy.init_node('joint_angles_publisher', anonymous=True)

        

        # 创建发布者

        self.pub = rospy.Publisher('joint_angles', JointAngles, queue_size=10)

        

        # 动态重配置服务器

        self.reconfigure_server = dynamic_reconfigure.server.Server(JointAnglesConfig, self.reconfigure_callback)

        

        # 默认发布频率

        self.rate = rospy.Rate(1)  # 1 Hz

    

    def reconfigure_callback(self, config, level):

        # 更新发布频率

        self.rate = rospy.Rate(config['publish_rate'])

        return config

    

    def publish_joint_angles(self):

        while not rospy.is_shutdown():

            # 创建消息实例

            joint_angles_msg = JointAngles()

            joint_angles_msg.shoulder = 0.5

            joint_angles_msg.elbow = 1.0

            joint_angles_msg.wrist = 1.5

            

            # 发布消息

            self.pub.publish(joint_angles_msg)

            

            # 打印日志

            rospy.loginfo("Published joint angles: shoulder=%f, elbow=%f, wrist=%f", 

                         joint_angles_msg.shoulder, joint_angles_msg.elbow, joint_angles_msg.wrist)

            

            # 等待下一次发布

            self.rate.sleep()



if __name__ == '__main__':

    publisher = JointAnglesPublisher()

    publisher.publish_joint_angles()

动态重配置的客户端示例


#!/usr/bin/env python

import rospy

from dynamic_reconfigure.msg import Config

from dynamic_reconfigure.srv import Reconfigure, ReconfigureRequest



def dynamic_reconfigure_client():

    # 初始化节点

    rospy.init_node('dynamic_reconfigure_client')

    

    # 等待服务可用

    rospy.wait_for_service('joint_angles_publisher_node/set_parameters')

    

    try:

        # 创建服务代理

        reconfigure = rospy.ServiceProxy('joint_angles_publisher_node/set_parameters', Reconfigure)

        

        # 创建请求

        req = ReconfigureRequest()

        config = Config()

        config.ints = []

        config.strings = []

        config.bools = []

        config.doubles = [{'name': 'publish_rate', 'value': 2.0}]

        req.config = config

        

        # 发送请求并接收响应

        res = reconfigure(req)

        

        # 处理响应

        if res.success:

            rospy.loginfo("Dynamic reconfigure succeeded: %s", res.result)

        else:

            rospy.logwarn("Dynamic reconfigure failed: %s", res.result)

    except rospy.ServiceException as e:

        rospy.logerr("Service call failed: %s", e)



if __name__ == '__main__':

    dynamic_reconfigure_client()

消息和服务的生命周期管理

ROS中的节点可以通过lifecycle功能来管理其生命周期。生命周期管理包括节点的启动、停止、暂停和恢复等状态。这在复杂系统中非常有用，可以确保节点在适当的时间执行适当的操作。

生命周期管理的节点示例


#!/usr/bin/env python

import rospy

import lifecycle_msgs.msg

import lifecycle_msgs.srv



class JointAnglesLifecycleNode:

    def __init__(self):

        # 初始化节点

        rospy.init_node('joint_angles_lifecycle_node', anonymous=True)

        

        # 创建发布者

        self.pub = rospy.Publisher('joint_angles', JointAngles, queue_size=10)

        

        # 创建服务

        self.service = rospy.Service('set_joint_angles', SetJointAngles, self.handle_set_joint_angles)

        

        # 创建生命周期管理服务

        self.lifecycle_service = rospy.Service('lifecycle', lifecycle_msgs.srv.Lifecycle, self.lifecycle_callback)

### 消息和服务的生命周期管理



ROS中的节点可以通过`lifecycle`功能来管理其生命周期。生命周期管理包括节点的启动、停止、暂停和恢复等状态。这在复杂系统中非常有用，可以确保节点在适当的时间执行适当的操作。



#### 生命周期管理的节点示例



以下是一个使用Python编写的生命周期管理节点示例，该节点管理机器人关节角度的发布和服务的生命周期：



```python

#!/usr/bin/env python

import rospy

import lifecycle_msgs.msg

import lifecycle_msgs.srv

from std_msgs.msg import Float64

from my_robot_msgs.msg import JointAngles

from my_robot_msgs.srv import SetJointAngles, SetJointAnglesResponse



class JointAnglesLifecycleNode:

    def __init__(self):

        # 初始化节点

        rospy.init_node('joint_angles_lifecycle_node', anonymous=True)

        

        # 创建发布者

        self.pub = rospy.Publisher('joint_angles', JointAngles, queue_size=10)

        

        # 创建服务

        self.service = rospy.Service('set_joint_angles', SetJointAngles, self.handle_set_joint_angles)

        

        # 创建生命周期管理服务

        self.lifecycle_service = rospy.Service('lifecycle', lifecycle_msgs.srv.Lifecycle, self.lifecycle_callback)

        

        # 当前关节角度

        self.current_joint_angles = JointAngles()

        self.current_joint_angles.shoulder = 0.0

        self.current_joint_angles.elbow = 0.0

        self.current_joint_angles.wrist = 0.0

        

        # 默认发布频率

        self.publish_rate = rospy.Rate(1)  # 1 Hz

        

        # 节点状态

        self.state = lifecycle_msgs.msg.State.IDLE



    def handle_set_joint_angles(self, req):

        # 处理请求

        rospy.loginfo("Setting joint angles: shoulder=%f, elbow=%f, wrist=%f", 

                     req.shoulder, req.elbow, req.wrist)

        

        # 更新当前关节角度

        self.current_joint_angles.shoulder = req.shoulder

        self.current_joint_angles.elbow = req.elbow

        self.current_joint_angles.wrist = req.wrist

        

        # 返回响应

        return SetJointAnglesResponse(True, "Joint angles set successfully")



    def publish_joint_angles(self):

        while not rospy.is_shutdown():

            if self.state == lifecycle_msgs.msg.State.ACTIVE:

                # 创建消息实例

                joint_angles_msg = JointAngles()

                joint_angles_msg.shoulder = self.current_joint_angles.shoulder

                joint_angles_msg.elbow = self.current_joint_angles.elbow

                joint_angles_msg.wrist = self.current_joint_angles.wrist

                

                # 发布消息

                self.pub.publish(joint_angles_msg)

                

                # 打印日志

                rospy.loginfo("Published joint angles: shoulder=%f, elbow=%f, wrist=%f", 

                             joint_angles_msg.shoulder, joint_angles_msg.elbow, joint_angles_msg.wrist)

            

            # 等待下一次发布

            self.publish_rate.sleep()



    def lifecycle_callback(self, req):

        # 处理生命周期请求

        if req.transition == lifecycle_msgs.srv.LifecycleRequest.TRANSITION_CONFIGURE:

            rospy.loginfo("Configuring node...")

            self.state = lifecycle_msgs.msg.State.UNCONFIGURED

            return lifecycle_msgs.srv.LifecycleResponse(True, "Node configured")

        

        elif req.transition == lifecycle_msgs.srv.LifecycleRequest.TRANSITION_ACTIVATE:

            rospy.loginfo("Activating node...")

            self.state = lifecycle_msgs.msg.State.ACTIVE

            return lifecycle_msgs.srv.LifecycleResponse(True, "Node activated")

        

        elif req.transition == lifecycle_msgs.srv.LifecycleRequest.TRANSITION_DEACTIVATE:

            rospy.loginfo("Deactivating node...")

            self.state = lifecycle_msgs.msg.State.INACTIVE

            return lifecycle_msgs.srv.LifecycleResponse(True, "Node deactivated")

        

        elif req.transition == lifecycle_msgs.srv.LifecycleRequest.TRANSITION_CLEANUP:

            rospy.loginfo("Cleaning up node...")

            self.state = lifecycle_msgs.msg.State.UNCONFIGURED

            return lifecycle_msgs.srv.LifecycleResponse(True, "Node cleaned up")

        

        else:

            rospy.logwarn("Unknown transition: %d", req.transition)

            return lifecycle_msgs.srv.LifecycleResponse(False, "Unknown transition")



if __name__ == '__main__':

    node = JointAnglesLifecycleNode()

    

    # 启动消息发布

    try:

        node.publish_joint_angles()

    except rospy.ROSInterruptException:

        pass

运行生命周期管理示例

启动ROS核心：


roscore

启动生命周期管理节点：


rosrun my_robot_package joint_angles_lifecycle_node.py

使用rosservice命令调用生命周期管理服务：


rosservice call /lifecycle "transition: 1"  # 配置节点

rosservice call /lifecycle "transition: 3"  # 激活节点

rosservice call /lifecycle "transition: 4"  # 停止节点

rosservice call /lifecycle "transition: 2"  # 清理节点

消息和服务的调试与监测

在开发过程中，调试和监测消息和服务的通信是非常重要的。ROS提供了多种工具来帮助开发者进行调试和监测。

使用`rostopic`和`rosservice`命令

查看主题列表：


rostopic list

查看消息类型：


rostopic type /joint_angles

查看消息内容：


rostopic echo /joint_angles

查看服务列表：


rosservice list

查看服务类型：


rosservice type /set_joint_angles

查看服务请求和响应格式：


rosservice args /set_joint_angles

使用`rqt`工具

rqt_console：显示节点的调试日志。


rosrun rqt_console rqt_console

rqt_logger_level：动态调整节点的日志级别。


rosrun rqt_logger_level rqt_logger_level

rqt_graph：显示节点和主题的通信图。


rosrun rqt_graph rqt_graph

rqt_plot：可视化消息数据。


rosrun rqt_plot rqt_plot

rqt_service_client：调用和可视化服务。


rosrun rqt_service_client rqt_service_client

消息和服务的最佳实践

消息类型选择：选择合适的消息类型，尽量使用标准消息类型，以提高代码的可读性和可维护性。
服务响应时间：确保服务响应时间合理，避免长时间阻塞。
错误处理：在节点中添加适当的错误处理机制，提高系统的健壮性。
代码结构：保持代码结构清晰，使用类和模块来组织代码。
文档和注释：编写详细的文档和注释，方便其他开发者理解和使用你的代码。

通过以上内容，你可以更好地理解和使用ROS中的消息和服务，实现机器人运动学仿真软件的二次开发。希望这些示例和工具能够帮助你在ROS开发中更加得心应手。