ros-robotics 原文存档
210922记录ros运行超声波过程 包含Twist概念
`root@clbrobot:/home/ubuntu# rosrun lbtopic2 teleop_twist_keyboard1sonar.py``#!/usr/bin/python # -*- coding: utf-8 -*- import time import RPi.GPIO as GPIO import numpy as np trigger_pin = 37 echo_pin = 35 GPIO.setmode(GPIO.BOARD) GPIO.setup(trigger_pin,GPIO.OUT) GPIO.setup(echo_pin,GPIO.IN) GPIO.setwarnings(False) def send_trigger_pulse(): GPIO.output(trigger_pin,True) time.sleep(0.0001) GPIO.output(trigger_pin,False) def wait_for_echo(value,timeout): count = timeout while GPIO.input(echo_pin) != value and count>0: count = count-1 def get_distance(): send_trigger_pulse() wait_for_echo(True,10000) start = time.time() wait_for_echo(False,10000) finish = time.time() pulse_len = finish-start distance_cm = pulse_len/0.000058 return distance_cm def lbmean(list,len): sum=0; #b=len(list) for i in range(0,len): sum+=list[i]; return(sum/(len*1.0)) def sonar_filter_distance(): each_value=[]#空列表 diff_value=[]#空列表，要计算差值 ev_local=0#最大偏差值的真实下标 df_local=0 output_value=0 #读8次 for i in range(1,9): each_value.append(get_distance()) each_value.sort()#排序 #print(each_value) #计算差值 for i in range(0,7): diff_value.append(each_value[i+1]-each_value[i]) #找到最大的差值在each_value的位置 df_local=diff_value.index(max(diff_value)) ev_local=df_local+1 #计算each_value的平均值 #avr_each=lbmean(each_value) # #判断最大差值的数据与平均值的关系 # if ev_local<4: # output_value=lbmean(each_value[(ev_local+1):],9-ev_local-1) # elif ev_local>4: # output_value=lbmean(each_value[:(ev_local)],ev_local) # else: # output_value=lbmean(each_value.pop(ev_local),8) # return output_value if ev_local<4: output_value=np.mean(each_value[(ev_local+1):]) elif ev_local>4: output_value=np.mean(each_value[:(ev_local)]) else: output_value=np.mean(each_value.pop(ev_local)) return output_value # while True: # print("cm = %f",sonar_filter_distance()) # time.sleep(1)``#!/usr/bin/env python import roslib; roslib.load_manifest('teleop_twist_keyboard') import rospy import hcsr04 import RPi.GPIO as GPIO 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(sys.stdin.fileno()) select.select([sys.stdin], [], [], 0) key = sys.stdin.read(1) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) return key def vels(speed,turn): return "currently:\tspeed %s\tturn %s " % (speed,turn) def sonar_distance(): return hcsr04.get_distance() if __name__=="__main__": settings = termios.tcgetattr(sys.stdin) pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1) rospy.init_node('teleop_twist_keyboard') speed = rospy.get_param("~speed", 0.5) turn = rospy.get_param("~turn", 1.0) x = 0 y = 0 z = 0 th = 0 status = 0 try: #GPIO.cleanup() print msg print vels(speed,turn) while(1): key = getKey() sonar_cm = sonar_distance() #50cm if sonar_cm<50: sonar_cm2 = sonar_distance() if sonar_cm2<50: key = '8' print("Dangerous distance is %f\t",sonar_cm2) # else: # key = getKey() # print("Safe distance isDangerous distance is %f\t",sonar_cm) 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 else: x = 0 y = 0 z = 0 th = 0 if (key == '\x03'): 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) GPIO.cleanup( (35, 37) ) except Exception,err: print(err) #GPIO.cleanup() 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) GPIO.cleanup( (35, 37) ) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)`ROS Twist
包括线速度xyz和角速度xyz