ROS_Python编程 之 案例代码核心解析（第一版）

ROS_Python编程 之 案例代码核心解析（第一版）

通过Handsfree mini机器人平台配套的中级教程，我对ros_python编程实现 传感器数据读取、运动控制 的知识做以下归纳：

1. 传感器数据读取

1.1 获取机器人底层硬件基本信息并打印

# 核心代码
#!/usr/bin/env
#coding=UTF-8 

import rospy
from handsfree_msgs.msg import robot_state

def callback(data): #回调函数
    rospy.loginfo("the embedded system_time: %fus",data.system_time) #下位机系统时间 
    rospy.loginfo("the embedded cpu temperature is: %f",data.cpu_temperature) #cpu温度
    rospy.loginfo("the battery voltage is: %f",data.battery_voltage) #电池电压
    rospy.loginfo("the battery power remain is: percent %f",data.power_remain*100) #电池电量剩余
    rospy.loginfo("——————————————————————————————————————— \n\r") 

def sub_state():
    rospy.init_node('sub_state', anonymous=True)
    rospy.Subscriber("/handsfree/robot_state", robot_state, callback) 
    rospy.spin()

if __name__ == '__main__':
    sub_state()

精髓：rospy.Subscriber("/handsfree/robot_state", robot_state, callback)

其中，话题订阅者Subscriber对象的构造函数的第二个参数是所要订阅的话题类型

1.2 获取IMU数据，并转化成欧拉角打印

 #!/usr/bin/env
 #coding=UTF-8

 import rospy
 import tf
 from tf.transformations import *
 from sensor_msgs.msg import Imu

 def callback(data):
     #这个函数是tf中的,可以将四元数转成欧拉角
     (r,p,y) = tf.transformations.euler_from_quaternion((data.orientation.x,data.orientation.y,data.orientation.z,data.orientation.w))
     #由于是弧度制，下面将其改成角度制看起来更方便
     rospy.loginfo("Pitch = %f,Roll = %f, Yaw = %f",r*180/3.1415926,p*180/3.1415926,y*180/3.1415926)

 def get_imu():
     rospy.init_node('get_imu', anonymous=True)
     rospy.Subscriber("/handsfree/imu", Imu, callback)
     rospy.spin()

 if __name__ == '__main__':
     get_imu()

精髓：(r,p,y)= tf.transformations.euler_from_quaternion((data.orientation.x,data.orientation.y,data.orientation.z,data.orientation.w))

其中，ros中欧拉角与四元数的互相转化可以如下实现：

# 欧拉角转四元数
pos = Pose()
q = tf.transformations.quaternion_from_euler(0, 0, point.z)
pos.orientation.x = q[0]
pos.orientation.y = q[1]
pos.orientation.z = q[2]
pos.orientation.w = q[3]

# 四元数转欧拉角
(r, p, y) = tf.transformations.euler_from_quaternion([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w])
sell.fill_euler_msg(msg, r, p, y)

1.3 获取激光雷达数据并打印

激光雷达数据是指雷达通过发射激光束探测目标的位置后，将接收到的从目标反射回来的信号（目标回波）与发射信号进行比较，作适当处理后，就可获得目标的有关信息。一般，激光雷达的驱动程序会将其封装好通过topic发布出来。

 #!/usr/bin/env
 #coding=UTF-8 

 import rospy
 from sensor_msgs.msg import LaserScan

 def callback(data):
     rospy.loginfo("the angle_min is %f",data.angle_min) #打印一些信息
     rospy.loginfo("the angle_max is %f",data.angle_max)
     rospy.loginfo("the scan data is ")
     for i in range(0, len(data.ranges)):
         print data.ranges[i]
     print "\n"

 def sub_state():
     rospy.init_node('sub_scan', anonymous=True)
     rospy.Subscriber("/scan", LaserScan, callback)
     rospy.spin()

 if __name__ == '__main__':
     sub_state()

1.4 获取摄像头数据，并用opencv_python库显示

摄像头数据就是图像，是指摄像头获取的彩色rgb图像和深度depth图像等，一般都将其封装在摄像头驱动之中，直接调用即可

 #!/usr/bin/env
 #coding=UTF-8

 import cv2
 import rospy
 from sensor_msgs.msg import Image
 from cv_bridge import CvBridge, CvBridgeError

 def callback(data):
     try:
         cv_image = CvBridge().imgmsg_to_cv2(data, "bgr8") #使用cv_bridge将其转化为mat类型
     except CvBridgeError as e:
         print(e)
     (rows,cols,channels) = cv_image.shape
     cv2.imshow("Image window", cv_image) #显示出图像
     cv2.waitKey(30) #30ms 后播放下一帧

 def get_photo():
     rospy.init_node('get_photo', anonymous=True)
     rospy.Subscriber("/camera/rgb/image_raw", Image, callback)
     rospy.spin() 
	   
 if __name__ == '__main__':
     get_photo()

精髓：cv_image = CvBridge().imgmsg_to_cv2(data, "bgr8")

其中，CvBridge用于连接ros信息和opencv信息

2. 控制机器人移动

2.1 控制机器人直线运动

handsfree mini 机器人工控机自带的ros系统一共有两份代码，分别是 linear_move.py 和 linear_move_by_srv.py

• linear_move.py 是通过改变代码的参数配置来传参
• linear_move_by_srv.py 是通过调用Service接口来传参

其中，linear_move.py 源码如下：

#!/usr/bin/env python
import tf
import math
import rospy
import geometry_msgs.msg

class LinearMove(object):
    def __init__(self):
        self.frame_base = rospy.get_param('~base_frame', '/base_link'