def callback(data): print 'received data: %f, %f' % (data.velocity, data.angle) msg_to_send = drive_values() msg_to_send.pwm_drive = convert(data.velocity) msg_to_send.pwm_angle = convert(data.angle) # print 'send: %d, %d' % (msg_to_send.pwm_drive, msg_to_send.pwm_angle) pub.publish(msg_to_send)
def shutdown(self): rospy.loginfo("Stopping the car") new_drive_values = drive_values() new_drive_values.pwm_drive = 9831 new_drive_values.pwm_angle = 9831 self.pub.publish(new_drive_values) rospy.sleep(1)
def angle_callback(data): angle = data.angle car_angle = angle msg = drive_values() pwm2 = arduino_map(angle,-100,100,6554,13108); msg.pwm_drive = car_velocity msg.pwm_angle = pwm2
def updatePWM(msg): #Mappa värden från drive_parameters (-100,100) till (6554,13108) #skulle kunna linj.int men vi vet att vi bara kommer att skicka -100, 0 och 100 til drive_parameters #-100 = 6554 #100 = 13108 #0=9831 vel = msg.velocity ang = msg.angle values = drive_values() if vel == 100: values.pwm_drive = 13108 - 3014 elif vel == -100: values.pwm_drive = 6554 + 2964 else: values.pwm_drive = 9831 if ang == 100: values.pwm_angle = 13108 - 1500 elif ang == -100: values.pwm_angle = 6554 + 1500 else: values.pwm_angle = 9831 pub.publish(values)
def talk(self): while not rospy.is_shutdown(): new_drive_values = drive_values() self.convert_to_pwm() new_drive_values.pwm_drive = self.pwm_drive new_drive_values.pwm_angle = self.pwm_angle self.pub.publish(new_drive_values) self.rate.sleep()
def velocity_callback(data): velocity = data.velocity car_velocity = velocity print("Velocity: ",velocity,"Angle: ",angle) # Do the computation pwm1 = arduino_map(velocity,-100,100,6554,13108); msg = drive_values() msg.pwm_drive = pwm1 msg.pwm_angle = car_angle pub.publish(msg)
def callback(data): velocity = data.velocity angle = data.angle print("Velocity: ", velocity, "Angle: ", angle) # Do the computation pwm1 = arduino_map(velocity, -100, 100, 6554, 13108) pwm2 = arduino_map(angle, -100, 100, 6554, 13108) msg = drive_values() msg.pwm_drive = pwm1 msg.pwm_angle = pwm2 pub.publish(msg)
def callback(data): velocity = data.velocity angle = data.angle print("Velocity: ",velocity,"Angle: ",angle) # Do the computation pwm1 = arduino_map(velocity,-100,100,6554,13108); pwm2 = arduino_map(angle,-100,100,6554,13108); msg = drive_values() msg.pwm_drive = pwm1 msg.pwm_angle = pwm2 pub.publish(msg)
def __init__(self): rospy.init_node('Talker', anonymous=False) self.cmd = drive_values() self.pub = rospy.Publisher('drive_pwm', drive_values, queue_size=10) self.em_pub = rospy.Publisher('eStop', Bool, queue_size=10) self.sub = rospy.Subscriber('drive_parameters', drive_param, self.update _cmd) self.rate = rospy.Rate(10) while not rospy.is_shutdown(): self.rate.sleep()
def loop(): while true: while velocity != 0 and velocity < 8: #if velocity < 8, then accelerate and sleep in a loop print("Velocity: ",8,"Angle: ",angle) # Do the computation pwm1 = arduino_map(8,-100,100,6554,13108); pwm2 = arduino_map(angle,-100,100,6554,13108); msg = drive_values() msg.pwm_drive = pwm1 msg.pwm_angle = pwm2 pub.publish(msg) scale = (-1/8)*x + 1 # a linear function which computes time to sleep timer.sleep(scale) print("Velocity: ",velocity,"Angle: ",angle) # Do the computation pwm1 = arduino_map(velocity,-100,100,6554,13108); pwm2 = arduino_map(angle,-100,100,6554,13108); msg = drive_values() msg.pwm_drive = pwm1 msg.pwm_angle = pwm2 pub.publish(msg)
def drive(): global input, brake # If the car needs to brake publish backwards and turn wheel to show active braking if brake == "Brake": pubBrake.publish(true) print("Emergency braking") temp = drive_values() temp.pwm_angle = 9831.0 temp.pwm_drive = 9700.0 rospy.loginfo(input) pub.publish(temp) elif brake == "NoBrake": pubBrake.publish(false) pub.publish(input)
def callback(data): global forward, left, pub, hostname rospy.loginfo("Callback is called stupid") rospy.loginfo(data) forward = maprange((-100, 100), (6554, 13108), data.velocity) left = maprange((-100, 100), (6554, 13108), data.angle) # response = os.system("ping -c 1 " + hostname) # if response == 0: # forward = 0 # left = 0 msg = drive_values() msg.pwm_drive = forward msg.pwm_angle = left pub.publish(msg)
def talker(): global v_prev rospy.init_node('serial_talker', anonymous=True) em_pub.publish(False) rospy.Subscriber("drive_param_fin", drive_param, callback_param) #v_prev = v = map_velocity(param_speed) curr_time = last_time = rospy.get_rostime() rate = rospy.Rate(20) while not rospy.is_shutdown(): # PWM pwm1 = arduino_map(param_speed, -100, 100, 6554, 13108) pwm2 = arduino_map(param_angle, -100, 100, 6554, 13108) msg = drive_values() msg.pwm_drive = pwm1 msg.pwm_angle = pwm2 # print '////////////////////param_speed: %f, param_angle: %f' % (param_speed, param_angle) # print '////////////////////pwm1: %f, pwm2: %f' % (msg.pwm_drive, msg.pwm_angle) pub.publish(msg) # Odometry curr_time = rospy.get_rostime() dt = float(curr_time.to_sec()) - float(last_time.to_sec()) debug_pub.publish("time: %f" % dt) yaw = map_angle(param_angle) debug_pub.publish("yaw: %f" % yaw) v = map_velocity(param_speed) debug_pub.publish("velocity: %f" % v) # dv = v - v_prev # debug_pub.publish("dv: %f" % dv) # ds = v*dt + 0.5*dv*dt # debug_pub.publish("distance inc: %f" % ds) odom_msg = get_odom(curr_time, v, yaw) # print '////////////////////odom x: %f, odom y: %f' % (odom_msg.twist.twist.linear.x, odom_msg.twist.twist.linear.y) # odom_msg = Odometry() odom_pub.publish(odom_msg) # v_prev = v last_time = curr_time rate.sleep()
def loop(): global velocity global angle while angle != 999: while angle != 999 and velocity != 0 and velocity <= 7: #if velocity < 8, then accelerate and sleep in a loop # Do the computation print("Velocity: ", velocity, "Angle: ", angle) pwm1 = arduino_map(8, -100, 100, 6554, 13108) pwm2 = arduino_map(angle, -100, 100, 6554, 13108) msg = drive_values() msg.pwm_drive = pwm1 msg.pwm_angle = pwm2 pub.publish(msg) time.sleep(.6) pwm1 = arduino_map(0, -100, 100, 6554, 13108) pwm2 = arduino_map(angle, -100, 100, 6554, 13108) msg = drive_values() msg.pwm_drive = pwm1 msg.pwm_angle = pwm2 pub.publish(msg) scale = float( (-.142) * velocity + 1.0) # a linear function which computes time to sleep if velocity >= 8: break time.sleep(scale) print("Velocity: ", velocity, "Angle: ", angle) # Do the computation pwm1 = arduino_map(velocity, -100, 100, 6554, 13108) pwm2 = arduino_map(angle, -100, 100, 6554, 13108) msg = drive_values() msg.pwm_drive = pwm1 msg.pwm_angle = pwm2 time.sleep(.05) pub.publish(msg) return
def drive(): global key, brake, obs i = 0 if brake == "brake": #ignore keyboard input print("Emergency braking") temp = drive_values() temp.pwm_angle = 11000.0 # tijdens het remmen wordt het wiel gedraait om actief remmen te tonen kan eruit temp.pwm_drive = 9200.0 # met deze waarde kan gespeeld worden lager getal is niet gelijk aan beter stoppen. laagste tot nu toe getest is 9200 print(temp) pub.publish(temp) elif brake == "NoBrake": #listen to keyboard input if obs == "clear": print("path is clear") print(key) pub.publish(key) elif key.pwm_drive < 9831: # driving backwards print("backwards") pub.publish(key) else: print("neutral") # car completely neutral temp = drive_values() temp.pwm_angle = 9831.0 temp.pwm_drive = 9831.0 pub.publish(temp)
def callback(drive_param_data): msg = drive_values() msg.pwm_drive = int((drive_param_data.velocity + 100) * 32.77 + 6554) msg.pwm_angle = int((drive_param_data.angle + 100) * 32.77 + 6554) print("---------") print("pwm_drive: " + str(msg.pwm_drive)) print("pwm_angle: " + str(msg.pwm_angle)) print("---------") print() pub.publish(msg) rate.sleep()
def auto_drive(): global car_run_speed global max_speed if car_run_speed < max_speed: car_run_speed += 0.01 drive_value = drive_values() drive_value.throttle = int(car_run_speed) drive_value.steering = 0.0 drive_values_pub.publish(drive_value) print("steer : ", drive_value.steering) print("throttle : ", drive_value.throttle)
def callback(data): msg = drive_values() velocity = data.x angle = data.y # forward and backward offset if (velocity < 0.0): velocity -= 10.1 elif (velocity > 0.0): velocity += 6.1 # Do the computation pwm1 = arduino_map(velocity, -100, 100, 6554, 13108) pwm2 = arduino_map(angle, -0.78, 0.78, 7619, 12836) msg.pwm_drive = pwm1 msg.pwm_angle = pwm2 print("pwm drive:", pwm1, "pwm angle:", pwm2) pub.publish(msg)
def callback(msg): global sec center = [] drive_value = drive_values() drive_value.throttle = int(10) drive_value.steering = (0) """ if msg.header.stamp.secs == sec: pass else: sec = msg.header.stamp.secs """ detected_obs = DetectedObstacles() true_obs = TrueObstacles() true_obs.detected = 0 true_obs_long = TrueObstacles() true_obs_long.detected = 0 rospy.loginfo(len(msg.circles)) for i in msg.circles: center.append([i.center.x, i.center.y]) if i.center.x < 5 and (i.center.y > -1.4 and i.center.y < 1.4): true_obs.detected = 1 if i.center.x < 8 and (i.center.y > -1.4 and i.center.y < 1.4): true_obs_long.detected = 1 point_obs = PointObstacles() point_obs.x = i.center.x point_obs.y = i.center.y point_obs.radius = i.radius point_obs.true_radius = i.true_radius detected_obs.obstacles.append(point_obs) if true_obs.detected: drive_value.throttle = int(0) # print("Stop!! \n") obstacles_pub.publish(detected_obs) trueObs_pub.publish(true_obs) trueObs_pub_long.publish(true_obs_long)
def callback(data): #pwm1 = data.velocity #pwm2 = data.angle velocity = data.velocity angle = data.angle print("Velocity: ", velocity, "Angle: ", angle) # Do the computation #if velocity > 1: # velocity = velocity + 7 #if velocity < -1: # velocity = velocity - 9 pwm1 = velocity #pwm1 = VelocityMap(velocity,-20,20); pwm2 = AngleMap(angle, -10, 10, 8200, 12000) msg = drive_values() msg.pwm_drive = pwm1 msg.pwm_angle = pwm2 pub.publish(msg)
def callback(data): velocity = data.velocity angle = data.angle # forward and backward offset if (velocity < 0): velocity -= 10.1 if (velocity > 0): velocity += 6.1 angle += 0 #print("Velocity: ",velocity,"Angle: ",angle) # Do the computation pwm1 = arduino_map(velocity, -100, 100, 6554, 13108) pwm2 = arduino_map(angle, -100, 100, 7619, 12836) msg = drive_values() msg.pwm_drive = pwm1 msg.pwm_angle = pwm2 print("pwm drive:", pwm1, "pwm angle:", pwm2) pub.publish(msg)
def auto_drive(pid): global car_run_speed global max_speed if car_run_speed <= max_speed: car_run_speed += 0.1 drive_value = drive_values() drive_value.throttle = int(4) #drive_value.throttle = 4 drive_value.steering = pid #drive_value.steering = pid drive_values_pub.publish(drive_value) print("steer : ", drive_value.steering) print("throttle : ", drive_value.throttle)
def auto_drive(pid, x_loc=None): global car_run_speed global max_speed w = 0 if car_run_speed < max_speed: car_run_speed += 0.01 drive_value = drive_values() drive_value.throttle = int(8) #drive_value.steering = tanh_preprocess(pid) #drive_value.steering = linear_preprocess(pid) drive_value.steering = linear_x_loc(x_loc) drive_values_pub.publish(drive_value) print("steer : ", drive_value.steering) print("throttle : ", drive_value.throttle)
def auto_drive(pid): global car_run_speed w = 0 if -0.065 < pid and pid < 0.065: w = 1 else: w = 0.3 if car_run_speed < 1.0 * w: car_run_speed += 0.002 * 10 else: car_run_speed -= 0.003 * 10 drive_value = drive_values() drive_value.throttle = int(3) drive_value.steering = (pid / 0.074) drive_values_pub.publish(drive_value) print('steer: ', drive_value.steering) print('speed: ', car_run_speed)
def auto_drive(pid): global car_run_speed global max_speed if car_run_speed > 5 and abs(pid) > 5: car_run_speed -= 0.5 elif car_run_speed <= max_speed: car_run_speed += 0.1 drive_value = drive_values() drive_value.throttle = int(car_run_speed) #drive_value.throttle = 4 drive_value.steering = -pid * 3 #drive_value.steering = pid drive_values_pub.publish(drive_value) print("real_steer : ", pid) print("return_steer : ", drive_value.steering) print("throttle : ", drive_value.throttle)
1. Subscribe to the keyboard messages (If you use the default keyboard.py, you must subcribe to "drive_paramters" which is publishing messages of "drive_param") 2. Map the incoming values to the needed PWM values 3. Publish the calculated PWM values on topic "drive_pwm" using custom message drive_values """ class TimeoutException(Exception): pass def timeout_handler(signum, frame): raise TimeoutException signal.signal(signal.SIGALRM, timeout_handler) varP = drive_values() varP.pwm_drive = 9830 varP.pwm_angle = 9830 def check_connection(): address = '192.168.1.1' signal.alarm(1) try: res = subprocess.call(['ping', '-c', '1', address]) return res == 0 except TimeoutException: return False def fnc_callback(msg):
#!/usr/bin/env python # # Copyright (c) 2018, University of Antwerp. # All rights reserved. # # Jens de Hoog, Stevie Maes, Bernd Smits, Jonas Vercauteren import rospy from std_msgs.msg import String from std_msgs.msg import Bool from race.msg import drive_param from race.msg import drive_values # initial values input = drive_values() input.pwm_angle = 9831.0 input.pwm_drive = 9831.0 brake = "NoBrake" # pub node is # - Publishing to the drive_pwm topic # - The message type are drive_values # - The queue has a size '10' which describes the limited amount of queued messages pub = rospy.Publisher('drive_pwm', drive_values, queue_size=10) # pubBrake node is # - Publishing to the eStop topic # - The message type is Bool # - The queue has a size '10' which describes the limited amount of queued messages pubBrake = rospy.Publisher('eStop', Bool, queue_size=10)
#!/usr/bin/env python import rospy from race.msg import drive_values from race.msg import angle_param from race.msg import velocity_param from std_msgs.msg import Bool import numpy as np angle = 0 velocity = 0 msg = drive_values() pub = rospy.Publisher('drive_pwm', drive_values, queue_size=10) em_pub = rospy.Publisher('eStop', Bool, queue_size=10) # function to map from one range to another, similar to arduino def arduino_map(x, in_min, in_max, out_min, out_max): return (x - in_min) * (out_max - out_min) // (in_max - in_min) + out_min # callback function on occurance of drive parameters(angle & velocity) def velCallback(data): global velocity velocity = data.velocity # Do the computation pwm1 = arduino_map(velocity,-100,100,6554,13108); msg.pwm_drive = pwm1 print("Velocity: ",velocity,"Angle: ",angle) pub.publish(msg) def angleCallback(data):
def callback(data): global input input = drive_values() input.pwm_angle = ((data.steer + 100) * 32.77) + 6554 input.pwm_drive = ((data.throttle + 100) * 32.77) + 6554 drive()
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Revision $Id$ ## Simple talker demo that published std_msgs/Strings messages ## to the 'chatter' topic import rospy from time import sleep from std_msgs.msg import String from race.msg import drive_param from race.msg import drive_values key = drive_values() key.pwm_angle = 9831.0 key.pwm_drive = 9831.0 obs = "clear" brake = "NoBrake" pub = rospy.Publisher('drive_pwm', drive_values, queue_size=10) def KeyboardData(data): global key key = drive_values() key.pwm_angle = ((data.steer + 100) * 32.77) + 6554 # min 6554 max 13108 key.pwm_drive = ((data.throttle + 100) * 32.77) + 6554 drive()
def KeyboardData(data): global key key = drive_values() key.pwm_angle = ((data.steer + 100) * 32.77) + 6554 # min 6554 max 13108 key.pwm_drive = ((data.throttle + 100) * 32.77) + 6554 drive()