ROSNOTE ： 键盘控制机器人运动

ROS与底盘的通信协议：

ROS平台与小车底盘通信一般是通过串口

写入串口的内容是 左右轮的速度

从串口中读取到的是小车x,y坐标，方向角，角速度

ROS平台串口处理程序：

主要是写在base_controller.cpp中，ROS向串口发送速度

实现键盘控制小车运动：

1、先学习一下这个控制原理

• 按下键盘，teleop_twist_keyboard 包会发布 /cmd_vel 主题发布速度
• 我们在 base_controller 节点订阅/cmd_vel话题，接收速度数据，转换成与底盘通信的格式，写入串口
• 我们在 base_controller 节点读取底盘向串口发送来的里程计数据，进行处理，然后再发布出去，同时更新tf
• 小车底盘接收到串口发送来的速度后，控制电机运转，实现键盘控制小车移动

2、具体操作过程参考https://www.ncnynl.com/archives/201703/1418.html

3、小车控制思想

 

 

代码解读（一般不用代码解读吧，都是官方的那个功能包，主要是连接到底盘）

功能：

1、键盘事件监控

2、速度反馈

3、主函数(主要实现控制)

创建两个字典，一个用于改变机器人运动方向,一个用于机器人速度控制

moveBindings = {

'i':(1,0),

'o':(1,-1),

'j':(0,1),

'l':(0,-1),

'u':(1,1),

',':(-1,0),

'.':(-1,1),

'm':(-1,-1),

}

 

speedBindings={

'q':(1.1,1.1),

'z':(.9,.9),

'w':(1.1,1),

'x':(.9,1),

'e':(1,1.1),

'c':(1,.9),

}

键盘的监控：

这里我们使用了select模块,termios模块和tty模块

select模块主要用于socket通信,见识文件描述的变化,完成非阻塞方式工作

termios模块为tty提供了一个IO控制的POSIX调用的接口

tty模块主要用于更改文件描述符fd的模式

首先我们要把输入文件描述符模式改为raw,然后直接调用操作系的IO接口,监控所有带fileno()方法的文件句柄

在这里,我们只需要读取一个字节的输入流

最后我们从属性设置文件描述符fd的tty属性,返回键值

def getKey():

     tty.setraw(sys.stdin.fileno())

     rlist, _, _ = select.select([sys.stdin], [], [], 0.1)

if rlist:

     key = sys.stdin.read(1)

else:

     key = ''

termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

return key

返回速度和旋转量的函数

def vels(speed,turn):

          return "currently:\tspeed %s\tturn %s " % (speed,turn)

最后是我们的主要的控制和逻辑函数

首先我们需要得带文件描述符fd的tty属性

其次就是发布速度

在后面就是对速度的一些控制代码

 

if __name__=="__main__":

      settings = termios.tcgetattr(sys.stdin)

     #节点名称

      rospy.init_node('mrobot_teleop')

    #定义在cmd_vel Topic中发布Twist消息，控制机器人速度

     pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

 

    #x为方向值

   x = 0

   th = 0

  status = 0

  count = 0

  acc = 0.1

target_speed = 0

target_turn = 0

control_speed = 0

control_turn = 0

try:

print msg

print vels(speed,turn)

while(1):

key = getKey()

# 运动控制方向键（1：正方向，-1负方向）

if key in moveBindings.keys():

x = moveBindings[key][0]

th = moveBindings[key][1]

count = 0

# 速度修改键

elif key in speedBindings.keys():

speed = speed * speedBindings[key][0] # 线速度增加0.1倍

turn = turn * speedBindings[key][1] # 角速度增加0.1倍

count = 0

 

print vels(speed,turn)

if (status == 14):

print msg

status = (status + 1) % 15

# 停止键

elif key == ' ' or key == 'k' :

x = 0

th = 0

control_speed = 0

control_turn = 0

else:

count = count + 1

if count > 4:

x = 0

th = 0

if (key == '\x03'):

break

 

# 目标速度=速度值*方向值

target_speed = speed * x

target_turn = turn * th

 

# 速度限位，防止速度增减过快

if target_speed > control_speed:

control_speed = min( target_speed, control_speed + 0.02 )

elif target_speed < control_speed:

control_speed = max( target_speed, control_speed - 0.02 )

else:

control_speed = target_speed

 

if target_turn > control_turn:

control_turn = min( target_turn, control_turn + 0.1 )

elif target_turn < control_turn:

control_turn = max( target_turn, control_turn - 0.1 )

else:

control_turn = target_turn

 

# 创建并发布twist消息

twist = Twist()

twist.linear.x = control_speed;

twist.linear.y = 0;

twist.linear.z = 0

twist.angular.x = 0;

twist.angular.y = 0;

twist.angular.z = control_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)

以下内容源于：

  https://blog.csdn.net/lizilpl/article/details/46757805

1、ROS Base Controller是ROS中的基本控制节点，其中包含基本PID算法来控制电机运动。它主要任务是监听终端中或其他节点发出的以/cmd_vel为topic的控制指令(Topic是ROS中的一个基本概念，不同的节点之间通过发布和订阅相同Topic的消息来通信)，然后发布/odom topic的odometry 消息(里程，指直接获得的机器人走过的距离，通常由编码器数据计算得到或结合惯导经EKF得到),同时发布/odom坐标系和机体坐标系之间的转换(如果odom数据由EKF得到的话，这个工作由 robot_pose_ekf 节点完成).

2、从终端发布twist消息控制机器人

它的Topic是/cmd_vel，base controler订阅Twist消息来控制电机。

 

---