class DBWNode(object): #------------------------------------------------------------- def __init__(self): rospy.init_node('dbw_node', log_level=rospy.DEBUG) self.twist_cmd = None self.twist_linear_vel = 0.0 self.twist_angular_vel = 0.0 self.velocity = None self.current_linear_vel = 0.0 self.current_angular_vel = 0.0 self.dbw_enabled = False self.waypoints = None # final waypoints self.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) self.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) self.brake_deadband = rospy.get_param('~brake_deadband', .1) self.decel_limit = rospy.get_param('~decel_limit', -5) self.accel_limit = rospy.get_param('~accel_limit', 1.) self.wheel_radius = rospy.get_param('~wheel_radius', 0.2413) self.wheel_base = rospy.get_param('~wheel_base', 2.8498) self.steer_ratio = rospy.get_param('~steer_ratio', 14.8) self.max_lat_accel = rospy.get_param('~max_lat_accel', 3.) self.max_steer_angle = rospy.get_param('~max_steer_angle', 8.) self.max_throttle = rospy.get_param('~max_throttle_proportional', 0.8) self.max_brake = rospy.get_param('~max_brake_proportional', 3250) rospy.logwarn("DBW: wheel_base=%f, max_steer_ratio=%f, max_throttle=%f, max_brake=%f", self.wheel_base, self.steer_ratio, self.max_throttle, self.max_brake) 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 `TwistController` object self.controller = Controller(self.wheel_base, self.steer_ratio, self.max_lat_accel, self.max_steer_angle) # TODO: Subscribe to all the topics you need to rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb) rospy.logwarn ("In DBWNode:__init__ Done!") self.loop() #--------------------------------------- # keep looping #--------------------------------------- def loop(self): # rospy.logdebug("In dbw_node:loop\n") rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): #------------------------------------------------- # TODO: # wait for current_vel & dbw_enabled from sim; # and proposed_linear from WPFollower #------------------------------------------------- if (self.current_linear_vel is 0.0) or (self.twist_linear_vel is 0.0) or (not self.dbw_enabled) : continue steer = self.controller.control_steer( self.twist_linear_vel, self.twist_angular_vel, self.current_linear_vel) throttle, brake = self.controller.control_throttle_brake( self.twist_linear_vel, self.current_linear_vel, self.max_throttle, self.max_brake) self.publish(throttle, brake, steer) # rospy.logwarn("DWB-loop: Published throttle: %f; brake: %f, steering: %f", throttle, brake, steer) rate.sleep() #----------------------------------------------------------------------- # callback for subscribing to dbw_enabled published msg by the simulator #----------------------------------------------------------------------- def dbw_enabled_cb(self, msg): self.dbw_enabled = bool(msg.data) # Enabled self-driving mode will publish throttle, steer and brake mode. if self.dbw_enabled: rospy.logwarn("**** ============================ ****") rospy.logwarn("**** Self-Driving Mode Activated ****") rospy.logwarn("**** ============================ ****") else: rospy.logwarn("**** ============================= ****") rospy.logwarn("**** Manual Driving Mode Activated ****") rospy.logwarn("**** ============================= ****") #---------------------------------------------------------------------------- # callback for subscribing to current_velocity msg published by the simulator #---------------------------------------------------------------------------- def current_velocity_cb(self, msg): self.velocity = msg.twist self.current_linear_vel = msg.twist.linear.x self.current_angular_vel = msg.twist.angular.z # rospy.logwarn("DBW:current_velocity_cb from simulator = %f", self.current_linear_vel) #--------------------------------------------------------------------- # callback for subscribing to twist_cmd published by Waypoint Follower #--------------------------------------------------------------------- def twist_cmd_cb(self, msg): self.twist_linear_vel = msg.twist.linear.x self.twist_angular_vel = msg.twist.angular.z # if self.twist_linear_vel > 0.5: # rospy.logwarn("DBW:twist_cmd_cb : proposed_linear vel = %f, proposed_angular vel = %f", # self.twist_linear_vel, self.twist_angular_vel) #-------------------------------------------------------------------- # called from loop #-------------------------------------------------------------------- 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)