python如何解析rosbag数据 ros python接口

自定义ROS服务，编写python server+client样例

• 1 资料
• 2 正文

• 2.1 基于turtlesim，写一个客户端，申请繁殖多个小乌龟
• 2.2 自定义服务，写server+client，实现两个整数求和
• 2.3 基于turtlesim，写server+client，控制小乌龟转向

• 3 总结

1 资料

本章节是本人 ROS高效入门第五章 – 自定义ROS服务，编写C++ server+client样例 的姊妹篇，使用 python 编写同样的三个样例，建议对比阅读，主要参考资料是一样的：
（1）《机器人操作系统（ROS）浅析》[美] Jason M. O’Kane 著 肖军浩 译，第8章
（2）ros Tutorials 初级教程的10，14~16节： ros Tutorials

2 正文

2.1 基于turtlesim，写一个客户端，申请繁殖多个小乌龟

（1）第一个样例基于turtlesim实现，只编写一个client，向turtlesim的/spawn服务发送请求，申请繁殖多个小乌龟。
创建spawn_turtle软件包和相关文件：

cd ~/catkin_ws/src/python
catkin_create_pkg spawn_turtle turtlesim roscpp rospy

mkdir launch scripts
touch launch/start.launch scripts/spawn_turtle.py

（2）编写spawn_turtle.py

#! /usr/bin/env python3
import rospy
import math
from turtlesim.srv import Spawn, SpawnRequest

def main():
    rospy.init_node("spawn_turtle")
    // 建立client句柄，等待/spawn服务
    client = rospy.ServiceProxy("spawn", Spawn)
    rospy.wait_for_service("spawn")

    for i in range(10):
        try:
            req = SpawnRequest()
            req.x = 1 + i
            req.y = 1 + i
            req.theta = math.pi/2
            req.name = "Leo%s" %i
			// 调用服务/spawn，传入req，返回resp
            resp = client(req)
            rospy.loginfo("spawned a turtle name %s" %resp.name)
        // 服务调用失败，用异常捕捉
        except rospy.ServiceException as e:
            rospy.loginfo("service call fail: %s" %e)

if __name__ == "__main__":
    main()

（3）编写CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(spawn_turtle)
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  turtlesim
)
catkin_package()
include_directories(
  ${catkin_INCLUDE_DIRS}
)
catkin_install_python(PROGRAMS
  scripts/spawn_turtle.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

（4）编写start.launch

<launch>
<node
    pkg="turtlesim"
    type="turtlesim_node"
    name="turtlesim"
    required="true"
/>
<node
    pkg="spawn_turtle"
    type="spawn_turtle.py"
    name="spawn_turtle_node"
    output="screen"
/>
</launch>

（5）编译并运行

cd ~/catkin_ws
catkin_make --source src/python/spawn_turtle/
source devel/setup.bash
roslaunch spawn_turtle start.launch

2.2 自定义服务，写server+client，实现两个整数求和

（1）此例主要是演示如何自定义服务，实现的功能很简单：client向server发送两个整数，server返回整数和。创建srv_self软件包和文件：

cd ~/catkin_ws/src/python
catkin_create_pkg srv_self message_generation rospy roscpp std_msgs message_runtime
mkdir launch srv scripts
touch launch/start.launch srv/AddTwoInts.srv scripts/server.py scripts/client.py

（2）编写AddTwoInts.srv，server.py和client.py
AddTwoInts.srv

int64 a
int64 b
---
int64 sum

server.py

#! /usr/bin/env python3

import rospy
// 导入AddTwoInts和对应的AddTwoIntsResponse，除此之外，还有AddTwoIntsRequest
from srv_self.srv import AddTwoInts, AddTwoIntsResponse
// 服务的回调函数，参数为AddTwoIntsRequest，返回是AddTwoIntsResponse
def add(req):
    resp = AddTwoIntsResponse()
    resp.sum = req.a + req.b
    rospy.loginfo("server: receive a = %ld, b = %ld, return sum = %ld" %(req.a, req.b, resp.sum))
    return resp

def main():
    rospy.init_node("add_server")
    // 建立server句柄，发布/add_ints服务，数据类型是AddTwoInts
    srv = rospy.Service("add_ints", AddTwoInts, add)
    rospy.loginfo("ready to add ints..")
    rospy.spin()

if __name__ == "__main__":
    main()

client.py

#! /usr/bin/env python3

import rospy
import random
from srv_self.srv import *

def main():
    rospy.init_node("add_client")
    // 建立client句柄，调用/add_ints服务
    client = rospy.ServiceProxy("add_ints", AddTwoInts)
    rospy.wait_for_service("add_ints")
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        try:
            req = AddTwoIntsRequest()
            req.a = random.randint(1, 100)
            req.b = random.randint(1, 100)
            // 调用服务
            resp = client(req)
            rospy.loginfo("client: send a = %ld, b = %ld, receive sum = %ld" %(req.a, req.b, resp.sum))
        except rospy.ServiceException as e:
            rospy.loginfo("service call fail: %s" %e)

        rate.sleep()

if __name__ == "__main__":
    main()

（3）编写CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(srv_self)
find_package(catkin REQUIRED COMPONENTS
  message_generation
  roscpp
  rospy
  std_msgs
)
add_service_files(
  FILES
  AddTwoInts.srv
)
generate_messages(
  DEPENDENCIES
  std_msgs
)
catkin_package()
include_directories(
  ${catkin_INCLUDE_DIRS}
)
catkin_install_python(PROGRAMS
  scripts/server.py scripts/client.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

（4）编写start.launch

<launch>
<node
    pkg="srv_self"
    type="server.py"
    name="add_server"
    respawn="true"
    output="screen"
/>
<node
    pkg="srv_self"
    type="client.py"
    name="add_client"
    required="true"
    output="screen"
/>
</launch>

（5）编译并运行

cd ~/catkin_ws
catkin_make --source src/python/srv_self/
source devel/setup.bash
roslaunch srv_self start.launch

2.3 基于turtlesim，写server+client，控制小乌龟转向

（1）本样例基于turtlesim，client向server申请修改全局控制量forward，server收到后，根据修改后的forward，向turtlesim_node发送新的运动命令，最终实现控制小乌龟转向。
创建toggle_forward软件包和文件：

cd ~/catkin_ws/src/python
catkin_create_pkg toggle_forward turtlesim geometry_msgs std_srvs roscpp rospy
mkdir launch scripts
touch launch/start.launch scripts/toggle_server.py scripts/toggle_client.py

（2）编写toggle_server.py和toggle_client.py
toggle_server.py

#! /usr/bin/env python3

import rospy
from geometry_msgs.msg import Twist
from std_srvs.srv import Empty, EmptyResponse
// 定义全量变量g_forward 
g_forward = True

def toggleForward(req):
    global g_forward
    // g_forward 取反
    g_forward = not g_forward
    fwd = "forward" if g_forward else "rotate"
    rospy.loginfo("now receiveing: %s" %fwd)
    // 返回空的EmptyResponse（不能像cpp中那样，返回true，运行会出错）
    return EmptyResponse()

def main():
    global g_forward

    rospy.init_node("toggle_server")
    // 建立server句柄，监听 /toggle_forward 服务
    srv = rospy.Service("toggle_forward", Empty, toggleForward)
    
    pub = rospy.Publisher("turtle1/cmd_vel", Twist, queue_size=10)
    rate = rospy.Rate(2)
    while not rospy.is_shutdown():
        msg = Twist()
        // 根据 g_forward 值，调整小乌龟运动方向
        msg.linear.x = 1.0 if g_forward else 0.0
        msg.angular.z = 0.0 if g_forward else 1.0
        pub.publish(msg)
        
        rate.sleep()

    rospy.spin()

if __name__ == "__main__":
    main()

toggle_client.py

#! /usr/bin/env python3
import rospy
from std_srvs.srv import Empty, EmptyRequest

def main():
    rospy.init_node("toggle_client")
    client = rospy.ServiceProxy("toggle_forward", Empty)
    rospy.wait_for_service("toggle_forward")
    rate = rospy.Rate(2)
    while True:
        try:
        	// 发送空的服务申请
            req = EmptyRequest()
            resp = client(req)
            rospy.loginfo("send toggle cmd")
        except rospy.ServiceException as e:
            rospy.loginfo("send toggle cmd failed: %s" %e)

        rate.sleep()

if __name__ == "__main__":
    main()

（3）编写CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(toggle_forward)
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  rospy
  std_srvs
  turtlesim
)
catkin_package()
include_directories(
  ${catkin_INCLUDE_DIRS}
)
catkin_install_python(PROGRAMS
  scripts/toggle_server.py scripts/toggle_client.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

（4）编写start.launch

<launch>
<node
    pkg="turtlesim"
    type="turtlesim_node"
    name="turtlesim"
    required="true"
/>
<node
    pkg="toggle_forward"
    type="toggle_server.py"
    name="toggle_forward_server"
    output="screen"
/>
<node
    pkg="toggle_forward"
    type="toggle_client.py"
    name="toggle_forward_client"
    output="screen"
/>
</launch>

（5）编译并运行

cd ~/catkin_ws
catkin_make --source src/python/toggle_forward/
source devel/setup.bash
roslaunch toggle_forward start.launch

3 总结

本文中的例子放在了本人的github上： ros_src