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()
