class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
VehicleMass = rospy.get_param('~vehicle_mass', 1736.35)
DecelerationLimit = rospy.get_param('~decel_limit', -5)
WheelRadius = rospy.get_param('~wheel_radius', 0.2413)
WheelBase = rospy.get_param('~wheel_base', 2.8498)
SteerRatio = rospy.get_param('~steer_ratio', 14.8)
MaxLateralAcceleration = rospy.get_param('~max_lat_accel', 3.)
MaxSteeringAngle = rospy.get_param('~max_steer_angle', 8.)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# TODO: Create `Controller` object
self.Controller = Controller(
VehicleMass=VehicleMass,
DecelerationLimit=DecelerationLimit,
WheelRadius=WheelRadius,
WheelBase=WheelBase,
SteerRatio=SteerRatio,
MaxLateralAcceleration=MaxLateralAcceleration,
MaxSteeringAngle=MaxSteeringAngle)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
self.CurrentVelocity = None
self.CurrentAngularVelocity = None
self.dbw_enabled = None
self.DesiredVelocity = None
self.DesiredAngularVelocity = None
self.Throttle = 0
self.SteeringAngle = 0
self.BreakPress = 0
self.loop()
def loop(self):
Freq = rospy.Rate(50)
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
if not None in (self.CurrentVelocity, self.DesiredVelocity,
self.DesiredAngularVelocity):
self.Throttle, self.BreakPress, self.SteeringAngle = self.Controller.Control(
self.DesiredVelocity, self.DesiredAngularVelocity,
self.CurrentVelocity, self.dbw_enabled)
if self.dbw_enabled:
self.publish(self.Throttle, self.BreakPress,
self.SteeringAngle)
Freq.sleep()
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
def twist_cb(self, msg):
self.DesiredVelocity = msg.twist.linear.x
self.DesiredAngularVelocity = msg.twist.angular.z
def velocity_cb(self, msg):
self.CurrentVelocity = msg.twist.linear.x
def publish(self, Throttle, BreakPress, SteeringAngle):
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 = SteeringAngle
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = BreakPress
self.brake_pub.publish(bcmd)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# TODO: Create `Controller` object
self.controller = Controller(vehicle_mass=vehicle_mass,
fuel_capacity=fuel_capacity,
brake_deadband=brake_deadband,
decel_limit=decel_limit,
accel_limit=accel_limit,
wheel_radius=wheel_radius,
wheel_base=wheel_base,
steer_ratio=steer_ratio,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
self.current_vel = None
self.curr_ang_vel = None
self.dbw_enabled = None
self.linear_vel = None
self.angular_vel = None
self.throttle = 0
self.steering = 0
self.brake = 0
self.loop()
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
if not None in (self.current_vel, self.linear_vel,
self.angular_vel):
self.throttle, self.brake, self.steering = self.controller.Control(
self.current_vel, self.dbw_enabled, self.linear_vel,
self.angular_vel)
if self.dbw_enabled:
self.publish(self.throttle, self.brake, self.steering)
rate.sleep()
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
def twist_cb(self, msg):
self.linear_vel = msg.twist.linear.x
self.angular_vel = msg.twist.angular.z
def velocity_cb(self, msg):
self.current_vel = msg.twist.linear.x
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)
