<s> import rospy <EOL> import math <EOL> from std_msgs . msg import String <EOL> from geometry_msgs . msg import Twist <EOL> key_mapping = { '<STR_LIT:w>' : [ <NUM_LIT:0> , <NUM_LIT:1> ] , '<STR_LIT:x>' : [ <NUM_LIT:0> , - <NUM_LIT:1> ] , <EOL> '<STR_LIT:a>' : [ <NUM_LIT:1> , <NUM_LIT:0> ] , '<STR_LIT:d>' : [ - <NUM_LIT:1> , <NUM_LIT:0> ] , <EOL> '<STR_LIT:s>' : [ <NUM_LIT:0> , <NUM_LIT:0> ] } <EOL> g_twist_pub = None <EOL> g_target_twist = None <EOL> g_last_twist = None <EOL> g_last_send_time = None <EOL> g_vel_scales = [ <NUM_LIT:0.1> , <NUM_LIT:0.1> ] <EOL> g_vel_ramps = [ <NUM_LIT:1> , <NUM_LIT:1> ] <EOL> def ramped_vel ( v_prev , v_target , t_prev , t_now , ramp_rate ) : <EOL> step = ramp_rate * ( t_now - t_prev ) . to_sec ( ) <EOL> sign = <NUM_LIT:1.0> if ( v_target > v_prev ) else - <NUM_LIT:1.0> <EOL> error = math . fabs ( v_target - v_prev ) <EOL> if error < step : <EOL> return v_target <EOL> else : <EOL> return v_prev + sign * step <EOL> def ramped_twist ( prev , target , t_prev , t_now , ramps ) : <EOL> tw = Twist ( ) <EOL> tw . angular . z = ramped_vel ( prev . angular . z , target . angular . z , t_prev , <EOL> t_now , ramps [ <NUM_LIT:0> ] ) <EOL> tw . linear . x = ramped_vel ( prev . linear . x , target . linear . x , t_prev , <EOL> t_now , ramps [ <NUM_LIT:1> ] ) <EOL> return tw <EOL> def send_twist ( ) : <EOL> global g_last_twist_send_time , g_target_twist , g_last_twist , g_vel_scales , g_vel_ramps , g_twist_pub <EOL> t_now = rospy . Time . now ( ) <EOL> g_last_twist = ramped_twist ( g_last_twist , g_target_twist , <EOL> g_last_twist_send_time , t_now , g_vel_ramps ) <EOL> g_last_twist_send_time = t_now <EOL> g_twist_pub . publish ( g_last_twist ) <EOL> def keys_cb ( msg ) : <EOL> global g_target_twist , g_last_twist , g_vel_scales <EOL> if len ( msg . data ) == <NUM_LIT:0> or not key_mapping . has_key ( msg . data [ <NUM_LIT:0> ] ) : <EOL> return <EOL> vels = key_mapping [ msg . data [ <NUM_LIT:0> ] ] <EOL> g_target_twist . angular . z = vels [ <NUM_LIT:0> ] * g_vel_scales [ <NUM_LIT:0> ] <EOL> g_target_twist . linear . x = vels [ <NUM_LIT:1> ] * g_vel_scales [ <NUM_LIT:1> ] <EOL> def fetch_param ( name , default ) : <EOL> if rospy . has_param ( name ) : <EOL> return rospy . get_param ( name ) <EOL> else : <EOL> print "<STR_LIT>" % ( name , default ) <EOL> return default <EOL> if __name__ == '<STR_LIT:__main__>' : <EOL> rospy . init_node ( '<STR_LIT>' ) <EOL> g_last_twist_send_time = rospy . Time . now ( ) <EOL> g_twist_pub = rospy . Publisher ( '<STR_LIT>' , Twist , queue_size = <NUM_LIT:1> ) <EOL> rospy . Subscriber ( '<STR_LIT>' , String , keys_cb ) <EOL> g_target_twist = Twist ( ) <EOL> g_last_twist = Twist ( ) <EOL> g_vel_scales [ <NUM_LIT:0> ] = fetch_param ( '<STR_LIT>' , <NUM_LIT:0.1> ) <EOL> g_vel_scales [ <NUM_LIT:1> ] = fetch_param ( '<STR_LIT>' , <NUM_LIT:0.1> ) <EOL> g_vel_ramps [ <NUM_LIT:0> ] = fetch_param ( '<STR_LIT>' , <NUM_LIT:1.0> ) <EOL> g_vel_ramps [ <NUM_LIT:1> ] = fetch_param ( '<STR_LIT>' , <NUM_LIT:1.0> ) <EOL> rate = rospy . Rate ( <NUM_LIT:20> ) <EOL> while not rospy . is_shutdown ( ) : <EOL> send_twist ( ) <EOL> rate . sleep ( ) </s>
