def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def throttle_cmd_publisher(self, throttle_value=0.0):
"""
ID = 98 in BUSCAN
# Throttle pedal
# Options defined below
float32 pedal_cmd
uint8 pedal_cmd_type
# Enable
bool enable
# Ignore driver overrides
bool ignore
# Watchdog counter (optional)
uint8 count
uint8 CMD_NONE=0
uint8 CMD_PEDAL=1 # Unitless, range 0.15 to 0.80
uint8 CMD_PERCENT=2 # Percent of maximum throttle, range 0 to 1
"""
#msg_str = "Publishing Throttle message in /vehicle/throttle_cmd %s" % rospy.get_time()
#rospy.loginfo(msg_str)
throttle_command_object = ThrottleCmd()
#throttle_command_object.pedal_cmd = random.uniform(0.15, 0.8)
throttle_command_object.pedal_cmd = throttle_value
throttle_command_object.pedal_cmd_type = 1
throttle_command_object.enable = False
throttle_command_object.ignore = False
throttle_command_object.count = 0
rospy.loginfo("Throttle Publish ::>" +
str(throttle_command_object.pedal_cmd))
self.pub_throttle_cmd.publish(throttle_command_object)
def publish(self, throttle, brake, steer):
## Function to publish the dbw commands, based on args
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
# rospy.logwarn("PUBLISH: throttle={:.2f}, brake={:.2f}, steering={:.2f}, dbw_enabled={:}".format(throttle, brake, steer, self.dbw_enabled))
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
#rospy.loginfo('TwistController: Steering = ' + str(steer))
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
"publish throttle, brake and steer commands, each to their corrresponding topics"
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
#rospy.logwarn("Throttle: {0}, Brake: {1}, Steer: {2}".format(throttle, brake, steer))
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def accelerate():
''' Throttle
# Throttle pedal
# Options defined below
float32 pedal_cmd
uint8 pedal_cmd_type
# Enable
bool enable
# Ignore driver overrides
bool ignore
# Watchdog counter (optional)
uint8 count
uint8 CMD_NONE=0
uint8 CMD_PEDAL=1 # Unitless, range 0.15 to 0.80
uint8 CMD_PERCENT=2 # Percent of maximum throttle, range 0 to 1
'''
throttle_msg = ThrottleCmd()
throttle_msg.pedal_cmd_type = 2
throttle_msg.pedal_cmd = 0.05 # 5 percent throttle
throttle_msg.enable = True
throttle_pub.publish(throttle_msg)
stop_msg = ThrottleCmd()
stop_msg.pedal_cmd_type = 2
stop_msg.pedal_cmd = 0.0 # 5 percent throttle
stop_msg.enable = True
print("Accelerate by " + str(throttle_msg))
throttle_pub.publish(throttle_msg)
sleep(1)
throttle_pub.publish(stop_msg)
print("Accelerartion stopped again")
def publish(self, throttle, brake, steer):
# rospy.loginfo("throttle: {0}, brake {1}, steer {2}, linear {3}, angular {4}".format(throttle, brake, steer, self.linear_vel, self.angular_vel))
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
# The numerical issue is taken care by brake_deadband
if brake > 0:
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_PERCENT
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
else:
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
def publish(self, throttle, brake, steer):
rospy.logdebug('publish with values throttle=%s, steer=%s , brake=,%s',
throttle, steer, brake)
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
# do not send brake command unless required
if brake > 0.001:
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
# Make sure not so brake and throttle at the same time
if brake > 0.0:
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
else:
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE # We want to command the brake in torque Nm
# If the requested torque is over the threshold power on the breake lights
bcmd.enable = brake > BrakeCmd.TORQUE_BOO
bcmd.pedal_cmd = brake # Set the brake value
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
rospy.loginfo("THROTTLE: {}, BRAKE: {}, STEER: {}\n".format(
throttle, brake, steer))
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def callback(self, data):
i = 0
try:
screen = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
screen = cv2.cvtColor(screen, cv2.COLOR_BGR2GRAY)
screen = cv2.resize(screen, (120, 160))
screen = cv2.normalize(screen,
screen,
alpha=0,
beta=1,
norm_type=cv2.NORM_MINMAX,
dtype=cv2.CV_32F)
print "test"
screen = np.array(screen).reshape(WIDTH, HEIGHT, 1)
predictionx = model.predict([screen])[0]
motion = predictionx[1]
turn = predictionx[0]
if 0.000001 < turn <= 0.01:
turn1 = 0
elif turn < 0.000001:
q = np.log10(turn)
turn1 = q * 0.2
elif 0.01 < turn <= 0.4:
turn = 1 / (turn * 10)
else:
turn1 = turn * 1.3
motion = motion * .15
self.pub_throttle_cmd = rospy.Publisher('/fusion/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.pub_steering_cmd = rospy.Publisher('/fusion/steering_cmd',
SteeringCmd,
queue_size=1)
throttle_command_object = ThrottleCmd()
throttle_command_object.pedal_cmd = motion
throttle_command_object.pedal_cmd_type = 1
throttle_command_object.enable = True
throttle_command_object.ignore = False
throttle_command_object.count = 0
self.pub_throttle_cmd.publish(throttle_command_object)
steering_command_object = SteeringCmd()
steering_command_object.steering_wheel_angle_cmd = turn1
steering_command_object.steering_wheel_angle_velocity = random.uniform(
0.0, 8.7)
steering_command_object.enable = True
steering_command_object.ignore = False
steering_command_object.quiet = False
steering_command_object.count = 0
self.pub_steering_cmd.publish(steering_command_object)
print turn, motion
def publish(self, throttle, brake, steer):
# Loggging ...
rospy.loginfo("CRTL T : %.2f, B : %.2f, S : %.2f", throttle, brake, steer)
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
#rospy.loginfo('Published throttle command: %f'%(throttle))
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
#rospy.logwarn('Published steering command: %f'%(steer))
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
#rospy.logerr('Published brake command: %f'%(brake))
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
rospy.loginfo(
'DBWNode: Publisher started with: throttle -> %.2f brake -> %.2f steer -> %.2f',
throttle, brake, steer)
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
# This function publishes throttle, brake, and steering
# Throttle values passed to publish should be in the range 0 to 1
# Brake values passed to publish should be in units of torque (N*m)
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publishThrottleCmd(self, pedalVal): msg = ThrottleCmd() msg.pedal_cmd = pedalVal msg.pedal_cmd_type = 2 ''' pedal_cmd_type=1: Unitless, range 0.15 to 0.50 =2: Percent of maximum throttle, range 0 to 1 ''' msg.enable = True msg.clear = False msg.ignore = False msg.count = 0 self.throttle_pub.publish(msg)
def publish(self, throttle, brake, steer):
'''
Function to convert values from controller to ROS messages
'''
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
rospy.logdebug('Throttle: {}'.format(throttle))
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
rospy.logdebug('Steer: {}'.format(steer))
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
rospy.logdebug('Brake: {}'.format(brake))
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
# Throttle values passed to publish are in the range 0 to 1.
# A throttle of 1 means the vehicle throttle will be fully engaged.
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
# Brake values passed to publish are in units of torque (N*m). See twist_controller for the math.
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
"""
Publish the throttle, brake, and steer command to the car while the DBW system is enabled
"""
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
rospy.loginfo("Velocity error: %s",
self.target_long_vel - self.cur_long_vel)
def publish(self, throttle, brake, steer):
rospy.loginfo(
'publishing dbw controller output : throttle %f, brake %f, steer %f',
throttle, brake, steer)
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
if self.dbw_enabled:
throttle, deceleration, steering = self.twist_controller.control(
self.current_velocity, self.desired_long_velocity,
self.desired_angular_velocity)
#rospy.logwarn('current_velocity: %f, desired_velocity: %f, throttle: %f, brake: %f',
# self.current_velocity, self.desired_long_velocity, throttle, deceleration)
# throttle message
throttle_msg = ThrottleCmd()
throttle_msg.enable = True
throttle_msg.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
throttle_msg.pedal_cmd = throttle # 0 ... 1
self.throttle_pub.publish(throttle_msg)
# brake message
brake_msg = BrakeCmd()
brake_msg.enable = True
brake_msg.pedal_cmd_type = BrakeCmd.CMD_TORQUE # Nm, range 0 to 3250
if deceleration > 0:
brake_msg.boo_cmd = True
brake_msg.pedal_cmd = self.deceleration_to_Nm(deceleration)
else:
brake_msg.boo_cmd = False
brake_msg.pedal_cmd = 0
self.brake_pub.publish(brake_msg)
# steering message
steering_msg = SteeringCmd()
steering_msg.enable = True
steering_msg.steering_wheel_angle_cmd = steering # in rad, range -8.2 to 8.2
self.steer_pub.publish(steering_msg)
rate.sleep()
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
''' brake command will set throttle value to 0, even if brake is zero'''
''' therefore the brake command is only sent if we are braking '''
if brake > 0:
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
#Normally a car is driven with either the gas or the brake pedal pressed. This way, the throttle command with steering will be sent OR the brake command with the steering. Never throttle and brake at the same time
if throttle != 0:
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
#rospy.loginfo("Throttle %f and Steering %f being published...", throttle, steer)
else:
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
#rospy.loginfo("Brake %f and Steering %f being published...", throttle, steer)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
