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)