补上ROS键盘遥控机器人的keys_to_twist_ramps.py文件

在 ROS键盘遥控机器人，通过参数服务器指定速度 文章介绍了集中键盘控制机器人运动的方式，其中keys_to_twist_ramps.py文件内容漏掉了，这里给补上，感谢一严发现这问题。

启动ROS系统，管理节点

roscore

启动机器人

roslaunch turtlebot3_gazebo turtlebot3_world.launch

keys话题捕获键盘敲击，按键“wxasd”

cd ~/mywanderbot_ws/src/mywanderbot/src
python key_publisher.py

显示输入的按键

rostopic echo keys

然后通过Twist来控制速度，订阅上面那个keys话题

cd ~/mywanderbot_ws/src/mywanderbot/src
python keys_to_twist.py

绘制线速度X跟角速度Z

rqt_plot cmd_vel/linear/x cmd_vel/angular/z

cd ~/mywanderbot_ws/src/mywanderbot/src
./keys_to_twist_ramps.py

绘制线速度X跟角速度Z

rqt_plot cmd_vel/linear/x cmd_vel/angular/z

 带参数的形式：

cd ~/mywanderbot_ws/src/mywanderbot/src
./keys_to_twist_ramps.py _linear_scale:=0.5 _angular_scale:=1.0 _linear_accel:=1.0 _angular_accel:=1.0

绘制线速度X跟角速度Z

rqt_plot cmd_vel/linear/x cmd_vel/angular/z

加上可执行的权限：chmod u+x keys_to_twist.py

keys_to_twist.py代码如下：

#!/usr/bin/env python
import rospy
import math
from std_msgs.msg import String
from geometry_msgs.msg import Twist

key_mapping={'w':[1,0],'x':[-1,0],'a':[0,1],'d':[0,-1],'s':[0,0]}
g_twist_pub=None
g_target_twist=None
g_last_twist=None
g_last_send_time=None
g_vel_scales=[0.1,0.1]
g_vel_ramps=[1,1]

def ramped_vel(v_prev,v_target,t_prev,t_now,ramp_rate):
    #compute maximum velocity step
    step=ramp_rate * (t_now-t_prev).to_sec()
    sign=1.0 if (v_target > v_prev) else -1.0
    v_change=math.fabs(v_target-v_prev)
    if v_change<step:
        return v_target
    else:
        return v_prev+sign*step

def ramped_twist(prev,target,t_prev,t_now,ramps):
    tw=Twist()
    tw.linear.x=ramped_vel(prev.linear.x,target.linear.x,t_prev,t_now,ramps[0])
    tw.angular.z=ramped_vel(prev.angular.z,target.angular.z,t_prev,t_now,ramps[1])
    return tw

def send_twist():
    global g_last_twist_send_time,g_target_twist,g_last_twist,g_vel_scales,g_vel_ramps,g_twist_pub
    t_now=rospy.Time.now()
    g_last_twist=ramped_twist(g_last_twist,g_target_twist,g_last_twist_send_time,t_now,g_vel_ramps)
    g_last_twist_send_time=t_now
    g_twist_pub.publish(g_last_twist)

def keys_cb(msg):
    global g_target_twist,g_last_twist,g_vel_scales
    if len(msg.data)==0 or not key_mapping.has_key(msg.data[0]):
        return
    vels=key_mapping[msg.data[0]]
    g_target_twist.linear.x=vels[0]*g_vel_scales[0]
    g_target_twist.angular.z=vels[1]*g_vel_scales[1]

def fetch_param(name,default):
    if rospy.has_param(name):
        return rospy.get_param(name)
    else:
        print('parameter [%s] not found,using %.3f' %(name,default))
        return default

if __name__=='__main__':
    rospy.init_node('keys_to_twist_ramps')
    g_last_twist_send_time=rospy.Time.now()
    g_twist_pub=rospy.Publisher('cmd_vel',Twist,queue_size=1)
    rospy.Subscriber('keys',String,keys_cb)
    g_target_twist=Twist()
    g_last_twist=Twist()
    g_vel_scales[0]=fetch_param('~linear_scale',0.1)
    g_vel_scales[1]=fetch_param('~angular_scale',0.1)
    g_vel_ramps[0]=fetch_param('~linear_accel',1.0)
    g_vel_ramps[1]=fetch_param('~angular_accel',1.0)
    rate=rospy.Rate(20)
    while not rospy.is_shutdown():
        send_twist()
        rate.sleep()