def publish(self, throttle, brake, steer):
rospy.loginfo(
'DBWNode::publish - publishing throttle (%s), brake (%s) and steer (%s) values twist_cmd x %s z %s', \
throttle, brake, steer, self.twist_cmd.twist.linear.x, self.twist_cmd.twist.angular.z)
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
# rospy.loginfo('DBWNode::publish - publishing throttle command')
if brake == 0.0 and throttle > 0.0:
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
# rospy.loginfo('DBWNode::publish - publishing steering command')
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.boo_cmd = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
# rospy.loginfo('DBWNode::publish - publishing brake command')
if brake != 0.0:
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
# Take care not to publish both throttle and brake
if throttle > 0:
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
if self.speed > 0.1:
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
rospy.loginfo('steer cmd: %s', steer)
if brake > 0:
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_PERCENT
# It's uncertain what these units should be. It looks like
# CMD_PERCENT is 0-1 for ThrottleCmd but 0-100 for BrakeCmd
# Further it looks like 100% is not a lot of braking at all
# So I've doubled that amount here to have a moderate amount
# of braking as observed in the simulator
bcmd.pedal_cmd = brake * 200
bcmd.boo_cmd = True
self.brake_pub.publish(bcmd)
def brake_on_off(brake_on):
''' Brake
# Brake pedal
# Options defined below
float32 pedal_cmd
uint8 pedal_cmd_type
# Brake On Off (BOO), brake lights
bool boo_cmd
# 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.50
uint8 CMD_PERCENT=2 # Percent of maximum torque, range 0 to 1
uint8 CMD_TORQUE=3 # Nm, range 0 to 3250
float32 TORQUE_BOO=520 # Nm, brake lights threshold
float32 TORQUE_MAX=3412 # Nm, maximum torque
'''
if not brake_on:
brake_msg = BrakeCmd()
brake_msg.pedal_cmd_type = 1
brake_msg.pedal_cmd = 0.15
brake_msg.boo_cmd = False
brake_msg.enable = True
brake_on = False
else:
brake_msg = BrakeCmd()
brake_msg.pedal_cmd_type = 1
brake_msg.pedal_cmd = 0.5
brake_msg.boo_cmd = True
brake_msg.enable = True
brake_on = True
brake_pub.publish(brake_msg)
print("Brake on " + str(brake_msg))
def timer_cmd(self, event):
if self.brake_cmd > 0:
msg = BrakeCmd()
msg.enable = True
msg.pedal_cmd_type = BrakeCmd.CMD_PEDAL
msg.pedal_cmd = self.brake_cmd
if msg.pedal_cmd > 0.2:
msg.boo_cmd = True
self.pub.publish(msg)
def publishBrakeCmd(self, pedalVal):
msg = BrakeCmd()
msg.pedal_cmd = pedalVal
msg.pedal_cmd_type = 2
msg.boo_cmd = False
msg.enable = True
msg.clear = False
msg.ignore = False
msg.count = 0
self.brake_pub.publish(msg)
def make_brake_msg(brake_value=0.0):
brake_command_object = BrakeCmd()
brake_command_object.pedal_cmd = brake_value
brake_command_object.pedal_cmd_type = 2
brake_command_object.boo_cmd = False
brake_command_object.enable = False
brake_command_object.ignore = False
brake_command_object.count = 0
return brake_command_object
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 loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
# TODO: Get predicted throttle, brake, and steering using `twist_controller`
# You should only publish the control commands if dbw is enabled
# throttle, brake, steering = self.controller.control(<proposed linear velocity>,
if self.dbw_enabled:
throttle, deceleration, steering = self.twist_controller.control(
self.current_velocity, self.desired_long_velocity,
self.desired_angular_velocity)
# 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_brake(self, brake_torque):
assert (0.0 <= brake_torque <= BrakeCmd.TORQUE_MAX)
if self.brake.update(brake_torque):
cmd = BrakeCmd()
cmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
cmd.pedal_cmd = brake_torque
cmd.boo_cmd = True
cmd.enable = True
#rospy.logerr('brake pedal_cmd is %f, enable %d', brake_torque, cmd.enable)
self.brake_pub.publish(cmd)
def brake_cmd_publisher(self, brake_value=0.0):
rospy.get_time()
brake_command_object = BrakeCmd()
brake_command_object.pedal_cmd = brake_value
brake_command_object.pedal_cmd_type = 1
brake_command_object.boo_cmd = True
brake_command_object.enable = True
brake_command_object.ignore = False
brake_command_object.count = 0
rospy.loginfo("Brake Publish ::>"+str(brake_command_object.pedal_cmd))
rospy.loginfo("Brake Pedal_cmd Report ::>"+str(self.brake_pedal_cmd))
rospy.loginfo("Brake Pedal_input Report ::>"+str(self.brake_pedal_input))
rospy.loginfo("Brake Pedal_output Report ::>"+str(self.brake_pedal_output))
self.pub_brake_cmd.publish(brake_command_object)
def publishBrakeCmd(self, pedalVal): msg = BrakeCmd() msg.pedal_cmd = pedalVal msg.pedal_cmd_type = 2 ''' pedal_cmd_type=1: Unitless, range 0.15 to 0.50 =2: Percent of maximum torque, range 0 to 1 =3: Torque (unit: N.m), range 0 to 3250 ''' msg.boo_cmd = False msg.enable = True msg.clear = False msg.ignore = False msg.count = 0 self.brake_pub.publish(msg)
def brake_cmd_publisher(self, brake_value=0.0):
"""
ID = 96 in BusCan
# Brake pedal
# Options defined below
float32 pedal_cmd
uint8 pedal_cmd_type
# Brake On Off (BOO), brake lights
bool boo_cmd
# 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.50
uint8 CMD_PERCENT=2 # Percent of maximum torque, range 0 to 1
uint8 CMD_TORQUE=3 # Nm, range 0 to 3250
float32 TORQUE_BOO=520 # Nm, brake lights threshold
float32 TORQUE_MAX=3412 # Nm, maximum torque
"""
#msg_str = "Publishing Brake message in /vehicle/brake_cmd %s" % rospy.get_time()
#rospy.loginfo(msg_str)
brake_command_object = BrakeCmd()
#brake_command_object.pedal_cmd = random.uniform(0.15, 0.5)
brake_command_object.pedal_cmd = brake_value
brake_command_object.pedal_cmd_type = 1
brake_command_object.boo_cmd = False
brake_command_object.enable = False
brake_command_object.ignore = False
brake_command_object.count = 0
rospy.loginfo("Brake Publish ::>" +
str(brake_command_object.pedal_cmd))
self.pub_brake_cmd.publish(brake_command_object)