ROS_Python编程 之 案例代码核心解析

本文主要是介绍ROS_Python编程 之 案例代码核心解析，对大家解决编程问题具有一定的参考价值，需要的程序猿们随着小编来一起学习吧！

ROS_Python编程 之 案例代码核心解析

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

文章目录

• • ROS_Python编程 之 案例代码核心解析
  • • 1. 传感器数据读取
    • • 1.1 获取机器人底层硬件基本信息并打印
      • 1.2 获取IMU数据，并转化成欧拉角打印
      • 1.3 获取激光雷达数据并打印
      • 1.4 获取摄像头数据，并用opencv_python库显示
    • 2. 控制机器人移动
    • • 2.1 控制机器人直线运动
      • 2.2 控制机器人转向运动
      • 2.3 编写一个键盘遥控程序
    • 3. 导航发送目标点

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')
        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()。

1. 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)

2. 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)

2.2 控制机器人转向运动

通过python程序控制机器人沿z轴左右转动即偏航，代码与直线运动代码差不多，handsfree mini 机器人工控机自带的ros系统一共有三份代码，分别是 radian_turn.py，radian_turn_by_srv.py 和 radian_turn_direct_by_srv.py

• radian_turn.py 是通过改变代码的参数配置来传参
• radian_turn_by_srv.py 和 radian_turn_direct_by_srv.py 是通过调用Service接口来传参
• radian_turn.py 和 radian_turn_by_srv.py 只会在-180~180度之间旋转（如:如果运行这两个代码，并传递参数6.28，机器人是不会动的，而运行 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可以旋转超过半圈，新增函数的功能是：如果需要旋转超过半圈，采用分两次旋转，即先旋转半圈，再旋转剩余的角度（备注：这只是一种理解方式，实际运行时机器人旋转是连贯的）

2.3 编写一个键盘遥控程序

到目前为止，我陆陆续续亲自接触过几台机器人小车，比如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),
	      }

# 精髓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

3. 导航发送目标点

通过ros的navigation接口向move_base导航系统发送goal，让机器人导航到目标点

这篇关于ROS_Python编程 之 案例代码核心解析的文章就介绍到这儿，希望我们推荐的文章对大家有所帮助，也希望大家多多支持为之网！