def mainloop():
'''
code for activate and deactivate the node
'''
global veloicty
global steer
# global motion_state
nodename = 'model_estimator'
mainloop.active = True
def signalResponse(data):
mainloop.active
if 'zero_monitor' in data.active_nodes:
if nodename in data.active_nodes:
mainloop.active = True
else:
mainloop.active = False
else:
rospy.signal_shutdown('no monitor')
rospy.Subscriber('/active_nodes', ActiveNode, signalResponse)
'''
...
'''
rospy.Subscriber("/imu_speed", ImuSpeed, icb)
rospy.Subscriber("/curvature", Curvature, ccb)
rospy.Subscriber("/velocity_level", VelocityLevel, vcb)
# rospy.Subscriber("/motion_state", MotionState, mcb)
pub = rospy.Publisher('/calibrated_control', Control, queue_size=10)
rospy.init_node(nodename, anonymous=True)
rate = rospy.Rate(10) # 10hz
control = Control()
control.is_auto = True
control.header.frame_id = 'car_frame'
i = 0
while not rospy.is_shutdown():
control.header.stamp = rospy.Time.now()
control.header.seq = i
i = i + 1
control.speed = velocity
control.steer = steer
#if motion_state == 'HALT':
# control.speed = 0.
if mainloop.active:
pub.publish(control)
rate.sleep()
def init():
pub = rospy.Publisher('control', Control, queue_size=10)
rospy.init_node('control_sender', anonymous=True)
rate = rospy.Rate(20)
msg = Control()
is_auto=1
estop=0
gear=0
speed=0 #m/s
steer=0 #degree
brake=1
while not rospy.is_shutdown():
key = getKey()
print("key" + key)
if key == 'q':
break
elif key == 'a':
steer = steer - 1
if steer <= -30 :
steer = -30
elif key == 'd':
steer = steer + 1
if steer >= 30 :
steer = 30
elif key == 'w':
speed = speed + 0.1
if speed >= 3:
speed = 3
elif key == 's':
speed = speed - 0.1
if speed < 0:
speed = 0
elif key == 'e':
if gear == 0:
gear = 2
elif gear == 2:
gear = 0
elif key == 'b':
if brake == 1:
brake = 50
elif brake == 50:
brake = 1
rospy.loginfo(msg)
msg.is_auto = is_auto
msg.estop = estop
msg.gear = gear
msg.brake = brake
msg.speed = round(speed,3)
msg.steer = round(steer,3)
msg.encoder = 100
pub.publish(msg)
rate.sleep()
def init():
pub = rospy.Publisher('/car_signal', Control, queue_size=10)
rospy.init_node('keyboard_input', anonymous=True)
rate = rospy.Rate(20)
msg = Control()
is_auto = 1
estop = 0
gear = 0
speed = rospy.get_param('/speed', 0) #m/s
steer = rospy.get_param('/steer', 0) #degree
brake = 0
time = rospy.get_param('/time', 0) + 0.3 #seconds
init = rospy.Time.now()
while not rospy.is_shutdown():
cur = rospy.Time.now()
if time != 0:
t = cur - init
key = getKey()
print("key" + key)
if key == 'q':
break
elif key == 'a':
steer = steer - 1
if steer <= -28:
steer = -28
elif key == 'd':
steer = steer + 1
if steer >= 28:
steer = 28
elif key == 'w':
speed = speed + 0.1
if speed >= 3:
speed = 3
elif key == 's':
speed = speed - 0.1
if speed < -3:
speed = -3
elif key == 'e':
if gear == 0:
gear = 2
elif gear == 2:
gear = 0
elif key == 'b':
if brake == 1:
brake = 50
elif brake == 50:
brake = 1
rospy.loginfo(msg)
print("steer_degree : ", steer)
msg.is_auto = is_auto
msg.estop = estop
msg.gear = gear
msg.brake = brake
msg.speed = round(speed, 3)
msg.steer = round(steer, 3)
msg.header.stamp = rospy.Time.now()
pub.publish(msg)
rate.sleep()