XIAOCHEJIANPAN

2021-11-30 21:01:31 阅读：200 来源： 互联网

标签：drive termios sys key msg XIAOCHEJIANPAN speed

#!/usr/bin/env python
import rospy

from ackermann_msgs.msg import AckermannDriveStamped

import sys, select, termios, tty

banner = """
1. Reading from the keyboard
2. Publishing to AckermannDriveStamped!
---------------------------
w
a s d
anything else : stop
CTRL-C to quit
"""

keyBindings = {
'w':(1,0),
'd':(1,-1),
'a':(1,1),
's':(-1,0),
}

def getKey():
   tty.setraw(sys.stdin.fileno())
   select.select([sys.stdin], [], [], 0)
   key = sys.stdin.read(1)
   termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
   return key

speed = 0.5
turn = 0.6

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

if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)
pub = rospy.Publisher("/ackermann_cmd_mux/output", AckermannDriveStamped,queue_size=1)
rospy.init_node('keyop')

x = 0
th = 0
status = 0

try:
    while(1):
       key = getKey()
       if key in keyBindings.keys():
          x = keyBindings[key][0]
          th = keyBindings[key][1]
       else:
          x = 0
          th = 0
          if (key == '\x03'):
             break
       msg = AckermannDriveStamped();
       msg.header.stamp = rospy.Time.now();
       msg.header.frame_id = "base_link";

       msg.drive.speed = x*speed;
       msg.drive.acceleration = 1;
       msg.drive.jerk = 1;
       msg.drive.steering_angle = th*turn
       msg.drive.steering_angle_velocity = 1

pub.publish(msg)

except:
print 'error'

finally:
    msg = AckermannDriveStamped();
    msg.header.stamp = rospy.Time.now();
    msg.header.frame_id = "base_link";

    msg.drive.speed = 0;
    msg.drive.acceleration = 1;
    msg.drive.jerk = 1;
    msg.drive.steering_angle = 0
    msg.drive.steering_angle_velocity = 1
    pub.publish(msg)

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

标签：drive,termios,sys,key,msg,XIAOCHEJIANPAN,speed
来源： https://blog.csdn.net/m0_53628874/article/details/121642510

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

ICode9

XIAOCHEJIANPAN