def initialize():
pub = rospy.Publisher('/control/command/normalized_wanted_speed', NormalizedSpeedCommand, queue_size=1)
steering = rospy.Publisher('/control/command/normalized_wanted_steering', NormalizedSteeringCommand, queue_size=1)
rospy.Subscriber("/carstate/steering", NormalizedSteeringCommand, queue_size=1)
rospy.Subscriber("/carstate/speed", Speed, callback)
rospy.Timer(5.0, timer)
h = Header()
h.stamp = rospy.Time.now()
mensaje = NormalizedSpeedCommand()
mensaje.header = h
mensaje.value = -0.45
pub.publish(mensaje)
def callback(data):
pub = rospy.Publisher('/control/command/normalized_wanted_speed',
NormalizedSpeedCommand,
queue_size=10)
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
mensaje = NormalizedSpeedCommand()
mensaje.header = h
mensaje.value = data.value
if data.value < 0:
mensaje.value = data.value * -1
if data.value > 0:
mensaje.value = data.value * -1
pub.publish(mensaje)
def backwards_right(data, date):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.value)
pub = rospy.Publisher('/control/command/normalized_wanted_speed', NormalizedSpeedCommand, queue_size=10)
steering = rospy.Publisher('/control/command/normalized_wanted_steering', NormalizedSteeringCommand, queue_size=10)
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
mensaje = NormalizedSpeedCommand()
mensaje.header = h
mensaje.value = data.value
j = std_msgs.msg.Header()
j.stamp = rospy.Time.now()
mensaje2 = NormalizedSteeringCommand()
mensaje2.header = j
mensaje2.value = date.value
if date.value > 0:
mensaje.vlaue = date.value * -1
pub.publish(mensaje)