通过Handsfree mini机器人平台配套的中级教程，我对ros_python编程实现 传感器数据读取、运动控制 的知识做以下归纳：
# 核心代码
#!/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对象的构造函数的第二个参数是所要订阅的话题类型
#!/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)
激光雷达数据是指雷达通过发射激光束探测目标的位置后，将接收到的从目标反射回来的信号（目标回波）与发射信号进行比较，作适当处理后，就可获得目标的有关信息。一般，激光雷达的驱动程序会将其封装好通过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()
摄像头数据就是图像，是指摄像头获取的彩色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信息
handsfree mini 机器人工控机自带的ros系统一共有两份代码，分别是 linear_move.py 和 linear_move_by_srv.py
其中，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')
self.frame_odom = rospy.get_param('~odom_frame', '/odom')
self.velocity_topic = rospy.get_param('~vel_topic', '/mobile_base/mobile_base_controller/cmd_vel')
self.tolerance_distance = rospy.get_param('~tolerance', 0.08) # m
self.speed_linear = rospy.get_param('~speed_linear', 0.1) # m/s
self.rate_pub = rospy.get_param('~velocity_pub_rate', 10) # 10Hz（一秒10次）
self.rate = rospy.Rate(self.rate_pub)
self.vel_pub = rospy.Publisher(self.velocity_topic,
geometry_msgs.msg.Twist,
queue_size=1)
self.tf_listener = tf.TransformListener()
rospy.on_shutdown(self.brake)
try:
self.tf_listener.waitForTransform(self.frame_odom,
self.frame_base,
rospy.Time(0),
rospy.Duration(5.0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logerr('tf catch exception when robot waiting for transform......')
exit(-1)
def move_to_target(self, dis_to_move=0):
"""
to make robot move forward/back dis_to_move meters
:param: dis_to_move: the distance make robot move, >0 means move forward, <0 means move back
:type: float
:return:
"""
self.robot_start_pos = self.__get_robot_pos()
rospy.logdebug("****************************************************************************")
rospy.logdebug("robot current position is x = %f, y = %f, try to move forward/back %f Meter"
%(self.robot_start_pos.x, self.robot_start_pos.y, dis_to_move))
rospy.logdebug("****************************************************************************")
while self.__is_robot_arrived(dis_to_move) is not True:
self.__move_robot(direction=(1 if dis_to_move > 0 else -1))
self.rate.sleep()
self.brake() # we have arrived the target position, so stop robot
rospy.loginfo('arrived the target point')
def __is_robot_arrived(self, dis_to_move):
"""
to check has the robot arrived target position
:param: dis_to_move: the distance thar robot needs to move forward/back
:type: float
:return: False --- robot has not arrived the target position
True --- robot has arrived the target position
"""
robot_cur_pos = self.__get_robot_pos()
dis_moved = math.sqrt(math.pow((robot_cur_pos.x - self.robot_start_pos.x), 2) +
math.pow((robot_cur_pos.y - self.robot_start_pos.y), 2))
dis_need_move = math.fabs(dis_to_move) - dis_moved
return False if math.fabs(dis_need_move) > self.tolerance_distance else True
def __move_robot(self, direction=1):
"""
send velocity to robot according to the direction
:param: direction: when direction = 1: make robot move forward
when direction = -1: make robot move back
:type: int
:return:
"""
move_cmd = geometry_msgs.msg.Twist()
move_cmd.linear.x = math.copysign(self.speed_linear, direction)
self.vel_pub.publish(move_cmd)
def __get_robot_pos(self):
"""
to get current position(x,y,z) of robot
:return: A geometry_msgs.msg.Point type store robot's position (x,y,z)
"""
try:
(trans, rot) = self.tf_listener.lookupTransform(self.frame_odom,
self.frame_base,
rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.logerr('tf catch exception when robot looking up transform')
exit(-1)
return geometry_msgs.msg.Point(*trans)
def brake(self):
"""
send command to stop the robot
:return:
"""
self.vel_pub.publish(geometry_msgs.msg.Twist())
if __name__ == '__main__':
rospy.init_node('LinearMove')
t_dis = rospy.get_param('~t_dis', 0.2) # m
if t_dis == 0.0:
rospy.logerr('no target distance set!')
exit(-1)
rospy.loginfo('try to move %f meters'%t_dis)
LinearMove().move_to_target(dis_to_move=t_dis)
# 精髓1 geometry_msgs.msg.Twist
self.vel_pub = rospy.Publisher(self.velocity_topic,geometry_msgs.msg.Twist,
queue_size=1)
geometry_msgs.msg.Twist 是一种消息类，原始定义如下：
Vector3 linear
Vector3 angular
紧凑定义如下：
geometry_msgs/Vector3 linear
geometry_msgs/Vector3 angular
# 精髓2 tf.TransformListener类
self.tf_listener = tf.TransformListener()
self.tf_listener.waitForTransform(self.frame_odom,
self.frame_base,
rospy.Time(0),
rospy.Duration(5.0))
通过监听tf，可以避免繁琐的旋转矩阵的计算，而直接获取相关信息。在监听中，最常用的两个函数是：tf.TransformListener.lookupTransform() 和 tf.TransformListener.transformPoint()。
- tf.TransformListener.lookupTransform()：
（1）功能：获得从第一个坐标系到第二个坐标系转变的位姿关系，包括旋转和平移；rospy.Time(0)指最近时刻存储的数据，不可以改成rospy.Time.now()，rospy.Time.now()指现在时刻，因为监听数据需要进行缓存，无法实时检测，会有几毫秒的延迟
（2）参数与返回值：第一个参数是第一个坐标系，第二个参数是第二个坐标系，第三个参数是需要进行转变的时间；返回值 (trans, rot) 分别是 translation平移 和 rotation旋转，其中rot是四元数(quaternion)形式
（3）实例函数：
# 获取机器人当前位姿的函数，其中self.tf_listener.lookupTransform(self.frame_odom,self.frame_base,rospy.Time(0))函数获取了odom与base_link转变的位姿，也即机器人较原点移动的位姿，从而获取当前状态 def __get_robot_pos(self): try: (trans, rot) = self.tf_listener.lookupTransform(self.frame_odom, self.frame_base, rospy.Time(0)) except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.logerr('tf catch exception when robot looking up transform') exit(-1) return geometry_msgs.msg.Point(*trans)
- tf.TransformListener.transformPoint()：
（1）功能：可以将一个坐标系下的点的坐标转换到另一个坐标系下
（2）参数：
# m_normal_pose的数据类型是geometry_msgs::PointStamped，需要定义m_normal_pose.header.frame_id即该点所属的坐标系 # "PTAM_world"则是坐标转换的结果，数据类型同样为geometry_msgs::PointStamped listener_.transformPoint("PTAM_world",m_normal_pose,pose_PTAM_world)；
另外，linear_move_by_srv.py 源码的精髓如下：
service_move_dis = rospy.Service('move_distance', handsfree_srv.SpecialMove, callback_linear_move)
通过python程序控制机器人沿z轴左右转动即偏航，代码与直线运动代码差不多，handsfree mini 机器人工控机自带的ros系统一共有三份代码，分别是 radian_turn.py，radian_turn_by_srv.py 和 radian_turn_direct_by_srv.py
其中，radian_turn.py 的主要函数如下：
def __get_robot_pos(self):
try:
trans, rot = self.tf_listener.lookupTransform(self.frame_odom,
self.frame_base,
rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.logerr('tf catch exception when robot looking up transform')
exit(-1)
return tf.transformations.euler_from_quaternion(rot)[2]
def __turn_robot(self, turn_direction=1):
"""
send velocity command to robot according to the direction
:param: direction: when direction = 1: make robot turn in clockwise
when direction = -1: make robot turn in counterclockwise
:type: int
:return:
"""
move_cmd = geometry_msgs.msg.Twist()
move_cmd.angular.z = math.copysign(self.speed_radian, turn_direction)
self.vel_pub.publish(move_cmd)
def turn_to_target(self, radian_to_turn=0.0):
# yaw [-pi,pi]->[0,2*pi)
robot_start_yaw = (self.__get_robot_pos() + math.pi*2) % (math.pi*2)
# return target_yaw = radian_to_turn + robot_start_yaw
target_yaw = math.copysign(math.fabs(radian_to_turn)%(math.pi*2), radian_to_turn)+robot_start_yaw
# transform the range of target_yaw
target_yaw = (target_yaw + math.pi*2) % (math.pi*2)
# to find the shortest direction to turn
radian_to_move = target_yaw - robot_start_yaw
if radian_to_move < -math.pi or math.pi < radian_to_move:
direction = 1 if radian_to_move < -math.pi else -1
else:
direction = 1 if radian_to_move > 0 else -1
self.brake() # to stop robot first
rospy.logdebug("****************************************************************************")
rospy.logdebug("the robot's Yaw = %f, try to turn to Yaw = %f, the direction = %d"
%(robot_start_yaw, target_yaw, direction))
rospy.logdebug("****************************************************************************")
while self.__is_robot_arrived(target_yaw) is not True:
self.__turn_robot(turn_direction=direction)
self.rate.sleep()
self.brake()
rospy.loginfo('arrived the target radian!')
知识学习：欧拉角的理解
旋转永远是做游戏的难点和混乱点，表示旋转有很多种方式，比如简单的欧拉角、较复杂的四元数、更复杂的矩阵。
欧拉角是表达旋转的最简单方式，形式上是一个三维向量，其值分别代表物体绕坐标系三轴（x，y，z）的旋转角度。显然，不同的旋转顺序定义会产生不同的旋转结果，所以一般引擎都会规定自己的旋转顺序。
绕三轴的旋转值pitch，yaw，roll说法来自航空界，分别翻译为：pitch（俯仰角），yaw（偏航角），roll（翻滚角），
注意：pitch，yaw，roll与x，y，z没有明确固定的对应顺序，只是人为定义而已，因此可能存在差异！！！
另外，pitch角度值介于[-90,90]【万向锁】，yaw角度值介于[-180,180]【越过180°、-180°前后，最近转动方向相反（参考下文精髓4配图）】
而 handsfree mini 机器人工控机自带ros系统中，target_yaw 对应 move_cmd.angular.z
# 精髓1 tf.transformations.euler_from_quaternion() return tf.transformations.euler_from_quaternion(rot)[2] #根据欧拉角列表下标，此处代码返回的是yaw轴角度值# 精髓2 def turn_to_target(self, radian_to_turn=0.0): robot_start_yaw = (self.__get_robot_pos() + math.pi*2) % (math.pi*2) # 解析： # yaw∈[-pi,pi], (yaw+2*pi)∈[pi,3*pi], (yaw+2*pi)%(2*pi)∈[0,2*pi)# 精髓3 def turn_to_target(self, radian_to_turn=0.0): target_yaw = math.copysign(math.fabs(radian_to_turn)%(math.pi*2), radian_to_turn)+robot_start_yaw # 解析1：math.fabs(radian_to_turn)%(math.pi*2) # radian_to_turn表示还需要转动的角度值（弧度制），math.fabs()取绝对值确保计算得到的角度值为正 # 解析2：math.copysign() # math.copysign(math.fabs(radian_to_turn)%(math.pi*2), radian_to_turn)返回一个浮点数，该浮点数由第一个参数math.fabs(radian_to_turn)%(math.pi*2)【即还需要转动的弧度制角度值（正角）】的值和第二个参数radian_to_turn【即还需要转动的弧度制角度值（正负不定）】的正负号组成# 精髓4 radian_to_move = target_yaw-robot_start_yaw if radian_to_move < -math.pi or math.pi < radian_to_move: direction = 1 if radian_to_move < -math.pi else -1 else: direction = 1 if radian_to_move > 0 else -1 # 解析见下图：
radian_turn_direct_by_srv.py 的精髓如下：
def callback_turn_radian(req):
radian_to_turn=req.target_radian_turn
while radian_to_turn > 3.14:
radian_to_turn = radian_to_turn - 3.14;
turn_radian.turn_to_target(3.14)
while radian_to_turn < -3.14:
radian_to_turn = radian_to_turn + 3.14;
turn_radian.turn_to_target(-3.14)
turn_radian.turn_to_target(radian_to_turn)
return handsfree_srv.SpecialTurnResponse(True)
# 解析：
# 由于yaw角度值介于[-180,180]【越过180°、-180°前后，最近转动方向相反】，故源码radian_turn.py和radian_turn_by_srv.py只能在-180~180度之间旋转，不能一次性旋转超过半圈；而源码radian_turn_direct_by_srv.py可以旋转超过半圈，新增函数的功能是：如果需要旋转超过半圈，采用分两次旋转，即先旋转半圈，再旋转剩余的角度（备注：这只是一种理解方式，实际运行时机器人旋转是连贯的）
到目前为止，我陆陆续续亲自接触过几台机器人小车，比如racecar、spark、mini等，在诸多源码中，键盘遥控程序是从不缺席的。反观这些源码，可以发现键盘遥控程序几乎都大同小异。因此，我在此注解一下 handsfree mini 机器人的键盘遥控程序，以后需要编写键盘遥控程序时均可参考。
#!/usr/bin/env python
import roslib
import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty
msg = """
Reading from the keyboard and Publishing to Twist!
---------------------------
Moving around:
u i o
j k l
m , .
For Holonomic mode (strafing), hold down the shift key:
---------------------------
U I O
J K L
M < >
t : up (+z)
b : down (-z)
anything else : stop
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
CTRL-C to quit
"""
moveBindings = {
'i':(1,0,0,0),
'o':(1,0,0,-1),
'j':(0,0,0,1),
'l':(0,0,0,-1),
'u':(1,0,0,1),
',':(-1,0,0,0),
'.':(-1,0,0,1),
'm':(-1,0,0,-1),
'O':(1,-1,0,0),
'I':(1,0,0,0),
'J':(0,1,0,0),
'L':(0,-1,0,0),
'U':(1,1,0,0),
'<':(-1,0,0,0),
'>':(-1,-1,0,0),
'M':(-1,1,0,0),
't':(0,0,1,0),
'b':(0,0,-1,0),
}
speedBindings={
'q':(1.1,1.1),
'z':(.9,.9),
'w':(1.1,1),
'x':(.9,1),
'e':(1,1.1),
'c':(1,.9),
}
def getKey():
# tty.setraw(fd, when=termios.TCSAFLUSH)将文件描述符fd的模式更改为raw。
# 如果when被省略，则默认为termios.TCSAFLUSH，并传递给termios.tcsetattr()
tty.setraw(sys.stdin.fileno())
# int select(int maxfdpl, fd_set *readset, fd_set *writeset, fd_set *exceptset, const struct timeval * timeout)，第一个参数是最大的文件描述符长度，第二个是需要监听可读的套接字, 第三个是需要监听可写的套接字, 第四个是需要监听异常的套接字, 第五个是时间限制设置。其他源码运用了该函数的返回值：如果监听的套接字满足了可读可写条件, 那么所返回的can_read 或 can_write就会有值, 然后就可以利用这些返回值进行后续操作
select.select([sys.stdin], [], [], 0)
key = sys.stdin.read(1)
# int tcsetattr(int fd, int optional_actions, const struct termios *termios_p)
# tcsetattr函数用于设置终端参数。函数运行成功时返回0，失败时返回-1
# 参数fd为打开的终端文件描述符，参数optional_actions用于控制修改起作用的时间，而结构体termios_p中保存了要修改的参数
# optional_actions可以取如下的值：TCSANOW(不等数据传输完毕就立即改变属性),TCSADRAIN(等待所有数据传输结束才改变属性),
# TCSAFLUSH：等待所有数据传输结束, 清空输入输出缓冲区才改变属性
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
speed = 0.30
turn = 0.6
def vels(speed,turn):
return "currently:\tspeed %s\tturn %s " % (speed,turn)
if __name__ == "__main__":
# sys.stdin表示标准化输入，termios.tcgetattr(fd)返回一个包含文件描述符fd的tty属性的列表
settings = termios.tcgetattr(sys.stdin)
pub = rospy.Publisher('/mobile_base/mobile_base_controller/cmd_vel', Twist, queue_size = 1)
rospy.init_node('teleop_twist_keyboard')
x = 0
y = 0
z = 0
th = 0
status = 0
try:
print msg
print vels(speed,turn)
while(1):
key = getKey()
if key in moveBindings.keys():
x = moveBindings[key][0]
y = moveBindings[key][1]
z = moveBindings[key][2]
th = moveBindings[key][3]
elif key in speedBindings.keys():
speed = speed * speedBindings[key][0]
turn = turn * speedBindings[key][1]
print vels(speed,turn)
if (status == 14):
print msg
status = (status + 1) % 15
x = 0
y = 0
z = 0
th = 0
else:
x = 0
y = 0
z = 0
th = 0
if (key == '\x03'): # ctrl+c
break
twist = Twist()
twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed;
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn #只有偏航角
pub.publish(twist)
except:
print e
finally:
twist = Twist()
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
pub.publish(twist)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
# 精髓1 用二维字典定义按键对应的运动信息（包括运动方向和速度大小）
moveBindings = {
'i':(1,0,0,0),
'o':(1,0,0,-1),
'j':(0,0,0,1),
'l':(0,0,0,-1),
'u':(1,0,0,1),
',':(-1,0,0,0),
'.':(-1,0,0,1),
'm':(-1,0,0,-1),
'O':(1,-1,0,0),
'I':(1,0,0,0),
'J':(0,1,0,0),
'L':(0,-1,0,0),
'U':(1,1,0,0),
'<':(-1,0,0,0),
'>':(-1,-1,0,0),
'M':(-1,1,0,0),
't':(0,0,1,0),
'b':(0,0,-1,0),
}
speedBindings={
'q':(1.1,1.1),
'z':(.9,.9),
'w':(1.1,1),
'x':(.9,1),
'e':(1,1.1),
'c':(1,.9),
}
# 解析：
# 经测试，用于定义按键对应运动"方向"信息的二维字典是任意赋值的，也就是说 +1、0、-1 是随机组合的（数字位数也没有限制），即便将各个按键对应的值全部改成(1,0,0,0)或(1,0,0)，机器人也可以正常运动，不会发生按键冲突
# 精髓2 python获取键盘按键信息
def getKey():
# tty.setraw(fd, when=termios.TCSAFLUSH)将文件描述符fd的模式更改为raw。
# 如果when被省略，则默认为termios.TCSAFLUSH，并传递给termios.tcsetattr()
tty.setraw(sys.stdin.fileno())
# 第一个参数是需要监听可读的套接字, 第二个是需要监听可写的套接字, 第三个是需要监听异常的套接字, 第四个是时间限制设置
# 如果监听的套接字满足了可读可写条件, 那么所返回的can_read 或 can_write就会有值, 然后就可以利用这些返回值进行后续操作
select.select([sys.stdin], [], [], 0)
key = sys.stdin.read(1)
# int tcsetattr(int fd, int optional_actions, const struct termios *termios_p)
# tcsetattr函数用于设置终端参数。函数运行成功时返回0，失败时返回-1
# 参数fd为打开的终端文件描述符，参数optional_actions用于控制修改起作用的时间，而结构体termios_p中保存了要修改的参数
# optional_actions可以取如下的值：TCSANOW(不等数据传输完毕就立即改变属性),TCSADRAIN(等待所有数据传输结束才改变属性),
# TCSAFLUSH：等待所有数据传输结束, 清空输入输出缓冲区才改变属性
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key