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) args = [vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle] self.controller = Controller(*args) # Subscriptions rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb, queue_size=1) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb, queue_size=1) rospy.Subscriber('/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1) # local variables self.dbw_enabled = True self.current_velocity = None self.twist_cmd = None self.loop()
def __init__(self): rospy.init_node('dbw_node') vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) # need this 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) # need this 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.) max_throttle = .4 # brakes are in units of Torque 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) 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, max_throttle=max_throttle) 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) # To compute the CTE rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb) rospy.Subscriber('/final_waypoints', Lane, self.waypoints_cb) self.cte_pub = rospy.Publisher('/vehicle/cte', Float32, queue_size=1) self.current_velocity = None self.current_ang_velocity = None self.dbw_enabled = None self.linear_velocity = None self.angular_velocity = None self.current_position = None self.waypoints = None self.loop()
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', 2.) max_steer_angle = rospy.get_param('~max_steer_angle', 5.) 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( YawController(wheel_base, steer_ratio, 0.001, max_lat_accel, max_steer_angle), wheel_radius * vehicle_mass) # min_speed = 0.001: in YawController.get_angle, it is divided by a radius that is # proportional to current speed. To avoid division by zero, we need a minimum speed. # Some important variables: self.dbw_enabled = False self.curr_linear_velocity = 0.0 self.curr_angular_velocity = 0.0 self.des_linear_velocity = 0.0 self.des_angular_velocity = 0.0 self.previous_timestamp = rospy.get_rostime().secs self.current_timestamp = 0.0 self.vel_cur = 0.0 self.delta_t = 0.0 # TODO: Subscribe to all the topics you need to rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb, queue_size=1) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb, queue_size=1) self.loop()
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) params = { '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 } self.controller = Controller(**params) self.dbw_enabled = False self.current_setpoint = None self.current_velocity = None self.final_waypoints = None self.current_pose = None rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb, queue_size=1) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb, queue_size=1) rospy.Subscriber('/final_waypoints', Lane, self.final_waypoints_cb) rospy.Subscriber('/current_pose', PoseStamped, self.current_pose_cb) self.loop()
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.) decel_limit = rospy.get_param('~max_lon_accel', 3.) 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) # NOTE: To prevent Carla from moving requires about 700 Nm of torque # (can be calculated from the `vehicle_mass`, `wheel_radius`, and desired # acceleration) # Create the `Controller` object self.controller = Controller(wheel_base, wheel_radius, steer_ratio, max_lat_accel, max_steer_angle, decel_limit, vehicle_mass) # Subscribe to all the necessary topics self.dbw_enabled_sub = rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) self.curr_velocity_sub = rospy.Subscriber('/current_velocity', TwistStamped, self.curr_velocity_cb) self.twist_cmd_sub = rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb) self.dbw_enabled = None # From '/vehicle/dbw_enabled' self.curr_velocity = None # From '/current_velocity' self.linear_vel = None # From '/twist_cmd' self.angular_vel = None # From '/twist_cmd' self.loop()
def __init__(self): rospy.init_node('dbw_node') # Save param from launch file as dictionary ros_param_dict = { 'vehicle_mass': rospy.get_param('~vehicle_mass', 1736.35), # must need for stoping the car 'fuel_capacity': rospy.get_param('~fuel_capacity', 13.5), # optional go precise with the weight '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), # must '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.) } # Subscribe to all the topics you need to rospy.Subscriber('/current_velocity', TwistStamped, self.cur_velocity_cb) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) # Publish topics. 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) self.controller = Controller(**ros_param_dict) # List of variables need to use self.dbw_enabled = None self.cur_linear_velocity = None # may need low pass filter to reduce noisy self.cur_angular_velocity = None self.target_linear_velocity = None self.target_angular_velocity = None # flag for checking msg received self.twist_cmd = False self.current_velocity = False self.loop()
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 `TwistController` object # self.controller = TwistController(<Arguments you wish to provide>) parms = { 'wheel_base': wheel_base, 'steer_ratio': steer_ratio, 'min_velocity': 0., 'max_lat_accel': max_lat_accel, 'max_steer_angle': max_steer_angle, 'decel_limit': decel_limit, 'accel_limit': accel_limit, 'deadband': brake_deadband } self.controller = Controller(**parms) self.current_command = None self.current_velocity = None self.dbw_enabled = False # TODO: Subscribe to all the topics you need to rospy.Subscriber('/twist_cmd', TwistStamped, self.callback_twist_cmd) rospy.Subscriber('/current_velocity', TwistStamped, self.callback_current_velocity) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.callback_dbw_enabled) rospy.Subscriber('/final_waypoints', Lane, self.final_waypoints_cb) self.loop() def final_waypoints_cb(self, msg): #should be OK self.final_waypoints = msg #rospy.logwarn(self.final_waypoints) #rospy.logwarn('dbw_node ' + str(self.final_waypoints.waypoints[1].twist.twist.linear.z)) #rospy.logwarn(self.final_waypoints.waypoints[1].twist.twist.linear.x) def loop(self): rate = rospy.Rate(20) # 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>, # <proposed angular velocity>, # <current linear velocity>, # <dbw status>, # <any other argument you need>) # if <dbw is enabled>: # self.publish(throttle, brake, steer) if (self.current_command is not None) and (self.current_velocity is not None): #current velocity, target velocity, and target angle and pass into control #rospy.logwarn('Setting Velocity') linear_target = self.current_command.twist.linear.x #rospy.logwarn(str(linear_target) ) angular_target = self.current_command.twist.angular.z linear_current = self.current_velocity.twist.linear.x angular_current = self.current_velocity.twist.angular.z throttle, brake, steering = self.controller.control( linear_target, angular_target, linear_current) #rospy.logwarn(str(throttle)) brake = brake * 500 #rospy.logwarn(str(brake)) # publish the control commands if dbw is enabled if self.dbw_enabled is True: self.publish(throttle, brake, steering) else: rospy.logwarn('resetting controller') self.controller.reset() 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) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) def callback_twist_cmd(self, msg): self.current_command = msg def callback_current_velocity(self, msg): self.current_velocity = msg def callback_dbw_enabled(self, msg): self.dbw_enabled = msg.data
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, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle, acc_kp = 1.5,acc_ki = 0.003, acc_kd = 3.5) # 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.cur_angle_vel = None self.linear_vel = None self.angular_vel = None self.dbw_enabled = None self.throttle = self.steering = self.brake = 0 # base_path = os.path.dirname(os.path.abspath(__file__)) # self.speedfile = os.path.join(base_path, 'speed.csv') # self.speed_data = [] self.loop() 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 if self.dbw_enabled and self.current_vel is not None: self.speed_data.append({'actual': self.current_vel, 'proposed': self.linear_vel}) def loop(self): rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): # TODO: Get predicted throttle, brake, and steering using `twist_controller` if None not in (self.current_vel, self.linear_vel, self.angular_vel): self.throttle, self.brake, self.steering = self.controller.controller(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 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)
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) args = [ vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle ] self.controller = Controller(*args) # Subscriptions rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb, queue_size=1) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb, queue_size=1) rospy.Subscriber('/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1) # local variables self.dbw_enabled = True self.current_velocity = None self.twist_cmd = None self.loop() def loop(self): rate = rospy.Rate(COMMAND_FREQUENCY) while not rospy.is_shutdown(): # Will ignore when we don't have info from `current_velocity`, `twist_cmd` and when dbw is not enabled if (self.twist_cmd is None) or (self.current_velocity is None) or (not self.dbw_enabled): continue throttle, brake, steering = self.controller.control( self.twist_cmd.twist.linear, self.twist_cmd.twist.angular, self.current_velocity.twist.linear) self.publish(throttle, brake, steering) 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) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) # Helper for subscriber below. def twist_cmd_cb(self, msg): """Subscribe to twist_cmd.""" self.twist_cmd = msg def current_velocity_cb(self, msg): """Subscribe to current_velocity_cb.""" self.current_velocity = msg def dbw_enabled_cb(self, msg): """Subscribe to dbw_enabled.""" self.dbw_enabled = msg
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", 0.1) decel_limit = rospy.get_param("~decel_limit", -5) accel_limit = rospy.get_param("~accel_limit", 1.0) 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.0) max_steer_angle = rospy.get_param("~max_steer_angle", 8.0) 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, queue_size=1) rospy.Subscriber("/twist_cmd", TwistStamped, self.twist_cb, queue_size=1) rospy.Subscriber("/current_velocity", TwistStamped, self.velocity_cb, queue_size=5) self.current_vel = None self.curr_ang_vel = None self.dbw_enabled = None self.linear_vel = None self.angular_vel = None self.throttle = self.steering = 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)
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 `TwistController` object # self.controller = TwistController(<Arguments you wish to provide>) self.car_controller = Controller(wheel_base, steer_ratio, 1., max_lat_accel, max_steer_angle) # TODO: Subscribe to all the topics you need to rospy.Subscriber('/twist_cmd', TwistStamped, self.command_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_state_cb) # Add other member variables self.current_velocity = 0 self.target_velocity = 0 self.current_angular_velocity = 0 self.target_angular_velocity = 0 self.dbw_state = False 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 # throttle, brake, steering = self.controller.control(<proposed linear velocity>, # <proposed angular velocity>, # <current linear velocity>, # <dbw status>, # <any other argument you need>) if self.dbw_state is True: throttle, brake, steering = self.car_controller.control( self.target_velocity, self.target_angular_velocity, self.current_velocity) self.publish(throttle, brake, steering) else: self.car_controller.speed_controller.reset() rate.sleep() def publish(self, throttle, brake, steer): if throttle is not 0: tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle self.throttle_pub.publish(tcmd) else: bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) scmd = SteeringCmd() scmd.enable = True scmd.steering_wheel_angle_cmd = steer self.steer_pub.publish(scmd) def command_cb(self, cmd): vx = cmd.twist.linear.x az = cmd.twist.angular.z self.target_velocity = vx self.target_angular_velocity = az def velocity_cb(self, vel): vx = vel.twist.linear.x az = vel.twist.angular.z self.current_velocity = vx self.current_angular_velocity = az def dbw_state_cb(self, dbw): self.dbw_state = dbw.data rospy.logwarn('dbw_state: %s', self.dbw_state)
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 `TwistController` object # self.controller = TwistController(<Arguments you wish to provide>) config = { '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 } self.controller = Controller(**config) self.is_dbw_enabled = False self.current_velocity = None self.proposed_velocity = None self.final_waypoints = None self.current_pose = None self.previous_loop_time = rospy.get_rostime() # TODO: Subscribe to all the topics you need to self.twist_sub = rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_message_callback, queue_size=1) self.velocity_sub = rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_callback, queue_size=1) self.dbw_sub = rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_callback, queue_size=1) self.final_wp_sub = rospy.Subscriber('final_waypoints', Lane, self.final_waypoints_cb, queue_size=1) self.pose_sub = rospy.Subscriber('/current_pose', PoseStamped, self.current_pose_cb, queue_size=1) 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 # throttle, brake, steering = self.controller.control(<proposed linear velocity>, <proposed angular velocity>, # <current linear velocity>, <dbw status>, <any other argument you need>) # if <dbw is enabled>: # self.publish(throttle, brake, steer) if (self.current_velocity is not None) and (self.proposed_velocity is not None) and (self.final_waypoints is not None): ros_time = rospy.get_rostime() ros_dura = ros_time - self.previous_loop_time dura_secs = ros_dura.secs + (1e-9 * ros_dura.nsecs) self.previous_loop_time = ros_time curr_lin_vel = self.current_velocity.twist.linear.x targ_lin_vel = self.proposed_velocity.twist.linear.x targ_ang_vel = self.proposed_velocity.twist.angular.z """ The final waypoints are used to fit a polynomial which represents the trajectory that the car is expected to take (1) The expected y value at the current x value of the car is evaluated by using the curve (2) This is compared with the current y value obtained from the car pose The difference between 1 and 2 is the CTE(Cross Track Error) We will transform world coordinates to car coordinates so that all x values are in the direction of the car and all y values represent lateral movement """ origin = self.final_waypoints[0].pose.pose.position # Given a list of waypoints, returns a list of [x,y] coordinates associated with those wp. wp_matrix = list( map( lambda waypoint: [ waypoint.pose.pose.position.x, waypoint.pose.pose. position.y ], self.final_waypoints)) # Convert the coordinates [x,y] in the world view to the car's coordinate. # Shift the points to the origin. shifted_matrix = wp_matrix - np.array([origin.x, origin.y]) # Derive an angle by which to rotate the points. offset = 11 angle = np.arctan2(shifted_matrix[offset, 1], shifted_matrix[offset, 0]) rotation_matrix = np.array([[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]]) rotated_matrix = np.dot(shifted_matrix, rotation_matrix) # Fit a 2 degree polynomial to the waypoints. degree = 2 coefficients = np.polyfit(rotated_matrix[:, 0], rotated_matrix[:, 1], degree) # Transform the current pose of the car to be in the car's coordinate system. shifted_pose = np.array([ self.current_pose.pose.position.x - origin.x, self.current_pose.pose.position.y - origin.y ]) rotated_pose = np.dot(shifted_pose, rotation_matrix) expected_y_value = np.polyval(coefficients, rotated_pose[0]) actual_y_value = rotated_pose[1] cross_track_err = expected_y_value - actual_y_value throttle, brake, steering = self.controller.control( targ_lin_vel, targ_ang_vel, curr_lin_vel, cross_track_err, dura_secs) if not self.is_dbw_enabled or abs( self.current_velocity.twist.linear.x) < 1e-5 and abs( self.proposed_velocity.twist.linear.x) < 1e-5: self.controller.reset() if self.is_dbw_enabled: self.publish(throttle, brake, steering) 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) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) def twist_message_callback(self, mesg): self.proposed_velocity = mesg def current_velocity_callback(self, mesg): self.current_velocity = mesg def dbw_enabled_callback(self, mesg): rospy.logwarn("DBW_ENABLED %s" % mesg) self.is_dbw_enabled = mesg.data def final_waypoints_cb(self, mesg): self.final_waypoints = mesg.waypoints def current_pose_cb(self, mesg): self.current_pose = mesg
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) params = { '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 } self.controller = Controller(**params) self.dbw_enabled = False self.current_setpoint = None self.current_velocity = None self.final_waypoints = None self.current_pose = None rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb, queue_size=1) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb, queue_size=1) rospy.Subscriber('/final_waypoints', Lane, self.final_waypoints_cb) rospy.Subscriber('/current_pose', PoseStamped, self.current_pose_cb) self.loop() def loop(self): rate = rospy.Rate(20) # 50Hz while not rospy.is_shutdown(): # You should only publish the control commands if dbw is enabled if (self.final_waypoints is not None) and (self.current_pose is not None) \ and (self.current_velocity is not None) and (self.current_setpoint is not None): fwp_size = len(self.final_waypoints.waypoints) final_waypoint1 = self.final_waypoints.waypoints[0] if fwp_size>0 else None final_waypoint2 = self.final_waypoints.waypoints[1] if fwp_size>1 else None current_location = self.current_pose.pose.position linear_setpoint = self.current_setpoint.twist.linear.x angular_setpoint = self.current_setpoint.twist.angular.z linear_current = self.current_velocity.twist.linear.x angular_current = self.current_velocity.twist.angular.z throttle, brake, steering = self.controller.control(linear_setpoint, angular_setpoint, linear_current, angular_current, self.dbw_enabled, final_waypoint1, final_waypoint2, current_location) if self.dbw_enabled: self.publish(throttle, brake, steering) 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) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) pass def dbw_enabled_cb(self, msg): self.dbw_enabled = msg.data pass def twist_cmd_cb(self, msg): self.current_setpoint = msg pass def current_velocity_cb(self, msg): self.current_velocity = msg pass def final_waypoints_cb(self, msg): self.final_waypoints = msg pass def current_pose_cb(self, msg): self.current_pose = msg pass
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node') vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) # need this 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) # need this 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.) max_throttle = .4 # brakes are in units of Torque 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) 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, max_throttle=max_throttle) 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) # To compute the CTE rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb) rospy.Subscriber('/final_waypoints', Lane, self.waypoints_cb) self.cte_pub = rospy.Publisher('/vehicle/cte', Float32, queue_size=1) self.current_velocity = None self.current_ang_velocity = None self.dbw_enabled = None self.linear_velocity = None self.angular_velocity = None self.current_position = None self.waypoints = None self.loop() def loop(self): rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): if self.dbw_enabled and None not in ( self.current_velocity, self.linear_velocity, self.angular_velocity): cte = 0. if None not in (self.current_position, self.waypoints): cte = self.calculate_cte() self.cte_pub.publish(cte) throttle, brake, steering = self.controller.control( self.current_velocity, self.linear_velocity, self.angular_velocity, cte) self.publish(throttle, brake, steering) rate.sleep() def dbw_enabled_cb(self, msg): self.dbw_enabled = msg def twist_cb(self, msg): self.linear_velocity = msg.twist.linear.x self.angular_velocity = msg.twist.angular.z def velocity_cb(self, msg): self.current_velocity = msg.twist.linear.x def pose_cb(self, msg): self.current_position = [msg.pose.position.x, msg.pose.position.y] def waypoints_cb(self, msg): self.waypoints = msg.waypoints def calculate_cte(self): def position(waypoint): return [waypoint.pose.pose.position.x, waypoint.pose.pose.position.y] # Get waypoint positions relative to the first positions = np.array( [position(waypoint) for waypoint in self.waypoints]) origin = positions[0] positions = positions - origin # Rotate the positions so that they are oriented in the direction of travel offset = 10 angle = np.arctan2(positions[offset, 1], positions[offset, 0]) rotation = np.array([ [np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)], ]) positions = np.dot(positions, rotation) # Transform the current pose of the car to be in the car's coordinate system translated = np.array(self.current_position) - origin rotated = np.dot(translated, rotation) # The CTE is simply the difference between the actual position and the expected position coefficients = np.polyfit(positions[:, 0], positions[:, 1], deg=2) expected = np.polyval(coefficients, rotated[0]) actual = rotated[1] return actual - expected 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)
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', 2.) max_steer_angle = rospy.get_param('~max_steer_angle', 5.) 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( YawController(wheel_base, steer_ratio, 0.001, max_lat_accel, max_steer_angle), wheel_radius * vehicle_mass) # min_speed = 0.001: in YawController.get_angle, it is divided by a radius that is # proportional to current speed. To avoid division by zero, we need a minimum speed. # Some important variables: self.dbw_enabled = False self.curr_linear_velocity = 0.0 self.curr_angular_velocity = 0.0 self.des_linear_velocity = 0.0 self.des_angular_velocity = 0.0 self.previous_timestamp = rospy.get_rostime().secs self.current_timestamp = 0.0 self.vel_cur = 0.0 self.delta_t = 0.0 # TODO: Subscribe to all the topics you need to rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb, queue_size=1) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb, queue_size=1) self.loop() def loop(self): rate = rospy.Rate(20) # 20Hz # counter = 0 # just for testing throttle _AND_ brake # des_linear_velocity = self.des_linear_velocity 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 current_time = rospy.get_rostime() current_secs = current_time.secs current_nsecs = current_time.nsecs self.current_timestamp = current_secs + current_nsecs/1000000000.0 self.delta_t = (self.current_timestamp - self.previous_timestamp) self.previous_timestamp = self.current_timestamp #self.dbw_enabled = True #if counter>100: # just for testing throttle _AND_ brake # counter = 0. # if des_linear_velocity == 0.: # des_linear_velocity = self.des_linear_velocity # else: # des_linear_velocity = 0. # rospy.logwarn("speed = "+str(des_linear_velocity)) if self.dbw_enabled: #self.controller.reload_params() throttle, brake, steering = self.controller.control( self.delta_t, self.des_linear_velocity, self.des_angular_velocity, self.curr_linear_velocity) self.publish(throttle, brake, steering) else: self.controller.init(self.curr_linear_velocity) rate.sleep() # counter = counter + 1 # just for testing throttle _AND_ brake 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 dbw_enabled_cb(self, msg): if (msg.data == True): self.dbw_enabled = True rospy.logwarn("DBW_ENABLED") else: self.dbw_enabled = False rospy.logwarn("DBW_DISABLED") def current_velocity_cb(self, message): # """From the incoming message extract the velocity message """ self.curr_linear_velocity = message.twist.linear.x self.curr_angular_velocity = message.twist.angular.z def twist_cmd_cb(self, message): # """From the incoming message extract the desired velocity (twist) message """ self.des_linear_velocity = message.twist.linear.x self.des_angular_velocity = message.twist.angular.z
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node') # rospy.logwarn("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 `TwistController` object self.controller = Controller(wheel_base, steer_ratio, max_lat_accel, max_steer_angle, vehicle_mass, wheel_radius, fuel_capacity, accel_limit, decel_limit) self.dbw_enabled = False self.cur_lin_vel = None self.cur_ang_vel = None self.des_lin_vel = None self.des_ang_vel = None # 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_smd_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.cur_velocity_cb) rospy.loginfo("DBW initialized!") self.loop() def loop(self): rate = rospy.Rate(50) # 50Hz # rospy.loginfo("Looping... state is: ") # rospy.loginfo(rospy.is_shutdown()) 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>, # <proposed angular velocity>, # <current linear velocity>, # <dbw status>, # <any other argument you need>) # self.cur_ang_vel = 0 # rospy.loginfo(self.cur_lin_vel) # rospy.loginfo(self.cur_ang_vel) # rospy.loginfo(self.des_lin_vel) # rospy.loginfo(self.des_ang_vel) if self.cur_lin_vel is None or self.cur_ang_vel is None or self.des_lin_vel is None or self.des_ang_vel is None: continue throttle, brake, steer = self.controller.control( self.dbw_enabled, self.cur_lin_vel, self.cur_ang_vel, self.des_lin_vel, self.des_ang_vel) # throttle = 1.0 # brake = 0.0 # steer = 0.0 if self.dbw_enabled: # rospy.logwarn("throttle= %s, brake= %s, steer= %s" , throttle, brake, steer) # rospy.logwarn("thr= %s, bra= %s", throttle, brake) self.publish(throttle, brake * 100, steer) 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) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) def dbw_enabled_cb(self, msg): self.dbw_enabled = msg.data pass def twist_smd_cb(self, msg): self.des_lin_vel = msg.twist.linear.x self.des_ang_vel = msg.twist.angular.z pass def cur_velocity_cb(self, msg): self.cur_lin_vel = msg.twist.linear.x self.cur_ang_vel = msg.twist.angular.z pass
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.is_dbw_enabled = False self.last_twist_command = None self.current_velocity = None self.current_pose = None self.final_waypoints = None self.previous_loop_time = rospy.get_rostime() self.previous_debug_time = rospy.get_rostime() self.throttle_pid = pid.PID(kp=10.0, ki=0.01, kd=1.0, mn=decel_limit, mx=0.5 * accel_limit) self.brake_pid = pid.PID(kp=120.0, ki=0.0, kd=1.0, mn=brake_deadband, mx=5000) self.steering_pid = pid.PID(kp=1.0, ki=0.1, kd=0.6, mn=-max_steer_angle, mx=max_steer_angle) 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.throttle_pid, self.brake_pid, self.steering_pid) # TODO: Subscribe to all the topics you need to rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_commands_cb, queue_size=1) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb, queue_size=1) rospy.Subscriber('/current_pose', geometry_msgs.msg.PoseStamped, self.current_pose_cb, queue_size=1) rospy.Subscriber('/final_waypoints', styx_msgs.msg.Lane, self.final_wp_cb, queue_size=1) self.loop()
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 `TwistController` object # self.controller = TwistController(<Arguments you wish to provide>) config = { '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 } self.controller = Controller(**config) self.is_dbw_enabled = False self.current_velocity = None self.proposed_velocity = None self.final_waypoints = None self.current_pose = None self.previous_loop_time = rospy.get_rostime() # TODO: Subscribe to all the topics you need to self.twist_sub = rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_message_callback, queue_size=1) self.velocity_sub = rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_callback, queue_size=1) self.dbw_sub = rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_callback, queue_size=1) self.final_wp_sub = rospy.Subscriber('final_waypoints', Lane, self.final_waypoints_cb, queue_size=1) self.pose_sub = rospy.Subscriber('/current_pose', PoseStamped, self.current_pose_cb, queue_size=1) self.loop()
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) # 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) # Subscribe to topics '/twist_cmd', '/current_velocity', '/vehicle/dbw_enabled' rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.current_vel_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enable_cb) self.current_vel = None # self.curr_ang_vel = None self.dbw_enabled = None self.linear_vel = None self.angular_vel = None self.throttle = self.steering = self.brake = 0 self.loop() def loop(self): # * Autoware safety feature, system auto shutdown when this is less than 10Hz rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): # Get predicted throttle, brake, and steering using `twist_controller` # Only publish the control commands when dbw_enabled is true to avoid error accumulation in manual mode 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 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 twist_cb(self, msg): self.linear_vel = msg.twist.linear.x self.angular_vel = msg.twist.angular.z def current_vel_cb(self, msg): self.current_vel = msg.twist.linear.x def dbw_enable_cb(self, msg): self.dbw_enabled = msg
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) self.proposed_linear_velocity = 0.0 self.proposed_angular_velocity = 0.0 self.current_linear_velocity = 0.0 self.current_angular_velocity = 0.0 self.dbw_enabled = False # TODO: Create `TwistController` object # self.controller = TwistController(<Arguments you wish to provide>) self.min_speed = 5 self.controller = Controller(wheel_base, steer_ratio, self.min_speed, max_lat_accel, max_steer_angle, accel_limit, decel_limit, vehicle_mass, wheel_radius, brake_deadband) # TODO: Subscribe to all the topics you need to rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb) # subscribing to /twist_cmd rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) 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 # throttle, brake, steering = self.controller.control(<proposed linear velocity>, # <proposed angular velocity>, # <current linear velocity>, # <dbw status>, # <any other argument you need>) # if <dbw is enabled>: # self.publish(throttle, brake, steer) throttle, brake, steering = self.controller.control( self.proposed_linear_velocity, self.proposed_angular_velocity, self.current_linear_velocity, self.dbw_enabled) if self.dbw_enabled: self.publish(throttle, brake, steering) rate.sleep() def twist_cmd_cb(self, twist_stamp): #rospy.logwarn('twist_cmd_handler: twist stamp = ' + str(twist_stamp)) self.proposed_linear_velocity = twist_stamp.twist.linear.x # Assumption: Only the z component of angular velocity is non-zero self.proposed_angular_velocity = twist_stamp.twist.angular.z def current_velocity_cb(self, curr_v): self.current_linear_velocity = curr_v.twist.linear.x self.current_angular_velocity = curr_v.twist.angular.z def dbw_enabled_cb(self, msg): self.dbw_enabled = msg.data 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 __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(<Arguments you wish to provide>) 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, queue_size=1) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb, queue_size=1) rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb, queue_size=1) self.dbw_enabled = False self.current_vel = None self.curr_ang_vel = None self.linear_vel = None self.angular_vel = None self.throttle = 0 self.steering = 0 self.brake = 0 self.loop()
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node', log_level=rospy.DEBUG) # Parameter 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.) min_speed = rospy.get_param('~min_speed', 1.) # Publisher self.steer_pub = rospy.Publisher('/vehicle/steering_cmd', SteeringCmd, queue_size=2) self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd', ThrottleCmd, queue_size=2) self.brake_pub = rospy.Publisher('/vehicle/brake_cmd', BrakeCmd, queue_size=2) # Controller controller_args = { 'wheel_base': wheel_base, 'steer_ratio': steer_ratio, 'min_speed': min_speed, 'max_lat_accel': max_lat_accel, 'max_steer_angle': max_steer_angle, 'decel_limit': decel_limit, 'accel_limit': accel_limit, 'vehicle_mass': vehicle_mass, 'wheel_radius': wheel_radius, 'brake_deadband': brake_deadband } self.controller = Controller(**controller_args) # Variables self.dbw_enabled = True # drive-by-wire is enabled by default and can be disable by a manual operator self.curr_vel = 0.0 # Current velocity self.curr_ang_vel = 0.0 # Current angular velocity self.target_vel = 0.0 # Target velocity self.target_ang_vel = 0.0 # Target angular velocity self.current_pose = None # current pose of the car self.final_waypoints = None # Lane object self.last_timestamp = rospy.rostime.get_time() self.curr_angle = 0.0 self.cte = 0.0 self.count = 0 # Subscriber rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1) rospy.Subscriber('/final_waypoints', Lane, self.final_waypoints_cb, queue_size=1) rospy.Subscriber('current_pose', PoseStamped, self.current_pose_cb, queue_size=1) rospy.Subscriber('/vehicle/steering_report', SteeringReport, self.current_angle_cb, queue_size=1) rospy.Subscriber('/vehicle/cte', Float32, self.cte_cb, queue_size=1) self.loop() def loop(self): rate = rospy.Rate(10) # 10Hz while not rospy.is_shutdown(): now = rospy.rostime.get_time() elapsed = now - self.last_timestamp self.last_timestamp = now # Saving Arguments for the Controller control_args = { 'target_vel': self.target_vel, # target linear velocity 'curr_vel': self.curr_vel, # current linear velocity 'target_ang_vel': self.target_ang_vel, # target angular velocity 'curr_ang_vel': self.curr_ang_vel, # current angular velocity 'dbw_enabled': self.dbw_enabled, # dbw status 'curr_angle': self.curr_angle, 'cte': self.cte, 'elapsed': elapsed } # Call Controller throttle, brake, angle = self.controller.control(**control_args) # We can uncomment the control calling above when they are done, first here some values for throttle, brake and angle # throttle = 1.0 # brake = 0 # angle = 0.0 if self.dbw_enabled: self.publish(throttle, brake, angle) rate.sleep() def publish(self, throttle, brake, steer): #debug_msg = "DBW T:{} \t B:{} \t S:{}\t".format(throttle, brake, steer) #rospy.logdebug(debug_msg) self.count += 1 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 dbw_enabled_cb(self, msg): self.dbw_enabled = bool(msg.data) def twist_cmd_cb(self, msg): self.target_vel = msg.twist.linear.x self.target_ang_vel = msg.twist.angular.z def current_velocity_cb(self, msg): self.curr_vel = msg.twist.linear.x self.curr_ang_vel = msg.twist.angular.z def current_pose_cb(self, msg): self.current_pose = msg.pose def cte_cb(self, msg): self.cte = msg.data def final_waypoints_cb(self, msg): self.final_waypoints_cb = msg.waypoints def current_angle_cb(self, msg): self.curr_angle = msg.steering_wheel_angle
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node') class Paramater_State(): vehicle_mass = None fuel_capacity = None brake_deadband = None decel_limit = None accel_limit = None wheel_radius = None wheel_base = None steer_ratio = None max_lat_accel = None max_steer_angle = None #Paramater States: S = Paramater_State() S.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) S.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) S.brake_deadband = rospy.get_param('~brake_deadband', .1) S.decel_limit = rospy.get_param('~decel_limit', -5) S.accel_limit = rospy.get_param('~accel_limit', 1.) S.wheel_radius = rospy.get_param('~wheel_radius', 0.2413) S.wheel_base = rospy.get_param('~wheel_base', 2.8498) S.steer_ratio = rospy.get_param('~steer_ratio', 14.8) S.max_lat_accel = rospy.get_param('~max_lat_accel', 3.) S.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 `TwistController` object self.dbw_enabled = True self.initial_time = rospy.get_time() self.pose = None self.twist_cmd = None self.reset = True self.waypoints = None self.current_velocity = None self.controller = Controller(S) # TODO: Subscribe to all the topics you need to rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_callback, queue_size=1) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_callback, queue_size=1) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_callback, queue_size=1) rospy.Subscriber('/current_pose', PoseStamped, self.pose_callback, queue_size=1) rospy.Subscriber('/final_waypoints', Lane, self.waypoints_callback, queue_size=1) self.loop() def loop(self): rate = rospy.Rate(100) # 50Hz #rate = rospy.Rate(20) # 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 time = rospy.get_time() delta_t = time - self.initial_time # Check time stamps. if time == self.initial_time: continue # Chck for divide by zero. #FIXME: Commenting for now, check why this is erroing delta_t = (time.nsec - self.initial_time.nsec) * 1e-6 #delta_t = (time - self.initial_time) * 1e-9 # Isn't nsec 9 zeros delta_t = (time - self.initial_time) * 1e-2 # Isn't nsec 9 zeros # Make sure there are enough waypoints. waypoints_in = self.waypoints is not None if not waypoints_in: continue #continue driving if less than 3 waypoints. #FIXME: -- commented the next 2 lines for now. ###if(len(self.waypoints) < 3): ### continue #Enable manual override and manual stop. #Reset first if self.dbw_enabled and self.twist_cmd is not None and self.current_velocity is not None and self.pose is not None: if self.reset: self.controller.reset() self.reset = False #Commands from twist controlller throttle, brake, steering = self.controller.control(self.twist_cmd, self.current_velocity, delta_t, self.pose, self.waypoints) rospy.loginfo("dbw_node: throttle: %f brake: %f steering: %f", throttle, brake, steering) else: self.reset = True throttle = 0.0 brake = 0.0 steering = 0.0 rospy.loginfo("dbw_node: reset") self.publish(throttle, brake, steering) 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) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) def twist_cmd_callback(self, msg): #self.twist_cmd = msg.twist self.twist_cmd = msg #Current_velocity callback comes from twist. def current_velocity_callback(self, msg): #self.current_velocity = msg.velocity self.current_velocity = msg #Pose callback comes from messages in pose. def pose_callback(self, msg): #self.pose = msg.pose self.pose = msg #DBW info comes in from data. def dbw_enabled_callback(self, msg): #self.dbw_enabled = msg.data self.dbw_enabled = msg #Populate waypoints def waypoints_callback(self, msg): #self.waypoints = msg.waypoints self.waypoints = msg
def __init__(self): rospy.init_node('dbw_node', log_level=rospy.DEBUG) # Parameter 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.) min_speed = rospy.get_param('~min_speed', 1.) # Publisher self.steer_pub = rospy.Publisher('/vehicle/steering_cmd', SteeringCmd, queue_size=2) self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd', ThrottleCmd, queue_size=2) self.brake_pub = rospy.Publisher('/vehicle/brake_cmd', BrakeCmd, queue_size=2) # Controller controller_args = { 'wheel_base': wheel_base, 'steer_ratio': steer_ratio, 'min_speed': min_speed, 'max_lat_accel': max_lat_accel, 'max_steer_angle': max_steer_angle, 'decel_limit': decel_limit, 'accel_limit': accel_limit, 'vehicle_mass': vehicle_mass, 'wheel_radius': wheel_radius, 'brake_deadband': brake_deadband } self.controller = Controller(**controller_args) # Variables self.dbw_enabled = True # drive-by-wire is enabled by default and can be disable by a manual operator self.curr_vel = 0.0 # Current velocity self.curr_ang_vel = 0.0 # Current angular velocity self.target_vel = 0.0 # Target velocity self.target_ang_vel = 0.0 # Target angular velocity self.current_pose = None # current pose of the car self.final_waypoints = None # Lane object self.last_timestamp = rospy.rostime.get_time() self.curr_angle = 0.0 self.cte = 0.0 self.count = 0 # Subscriber rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1) rospy.Subscriber('/final_waypoints', Lane, self.final_waypoints_cb, queue_size=1) rospy.Subscriber('current_pose', PoseStamped, self.current_pose_cb, queue_size=1) rospy.Subscriber('/vehicle/steering_report', SteeringReport, self.current_angle_cb, queue_size=1) rospy.Subscriber('/vehicle/cte', Float32, self.cte_cb, queue_size=1) self.loop()
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.cont = 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, speed_kp=2., accel_kp=0.4, accel_ki=0.1, accel_tau=0.5) 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 = TwistController(<Arguments you wish to provide>) # TODO: Subscribe to all the topics you need to rospy.Subscriber('/vehicle/steering_report', SteeringReport, self.vel_cb) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) self.throttle = self.brake = self.steer = 0. self.current_velocity = self.linear = self.angular = None self.dbw_enabled = False self.loop() def loop(self): rate = rospy.Rate(50) while not rospy.is_shutdown(): if not None in (self.current_velocity, self.linear, self.angular): t, b, s = self.cont.control(self.linear, self.angular, self.current_velocity, self.dbw_enabled) self.throttle, self.brake, self.steer = t, b, s print(t) if self.dbw_enabled: self.publish(self.throttle, self.brake, self.steer) 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) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) def vel_cb(self, msg): self.current_velocity = msg.speed def twist_cb(self, msg): self.linear = msg.twist.linear.x self.angular = msg.twist.angular.z def dbw_enabled_cb(self, msg): self.dbw_enabled = msg.data
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node') # Receive configuration parameters 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.) # Create publishers 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(<Arguments you wish to provide>) 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) # init variables self.current_vel = None self.curr_ang_vel = None self.dbw_enabled = None self.target_linear_vel = None self.target_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.target_linear_vel, self.target_angular_vel): self.throttle, self.brake, self.steering = self.controller.control( self.current_vel, self.dbw_enabled, self.target_linear_vel, self.target_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): #Only x-direccion linear and z-direcction angular are nonzero. Linear is already m/s and angular in rad/s. self.target_linear_vel = msg.twist.linear.x self.target_angular_vel = msg.twist.angular.z #rospy.loginfo("dbw_node: Received /twist_cmd (linear speed={0} m/s, angular speed={1} rad/s)".format(self.target_linear_vel, self.target_angular_vel)) def velocity_cb(self, msg): # Coordinates are vehicle centered. Only the x-direction linear velocity is nonzero. The speed is already in m/s. self.current_vel = msg.twist.linear.x #rospy.loginfo("dbw_node: Received /current_velocity (speed={0} m/s)".format(self.current_vel)) 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)
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) 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) rospy.Subscriber('/current_velocity', TwistStamped, self.current_vel_cb) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) self.current_velocity = None self.req_linear_velocity = None self.req_angular_velocity = None self.dbw_enabled = False self.throttle = 0 self.brake = 0 self.steer = 0 self.loop() def loop(self): rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): if not None in (self.current_velocity, self.req_linear_velocity, self.req_angular_velocity): self.throttle, self.brake, self.steer = self.controller.control(\ self.current_velocity, self.req_linear_velocity, self.req_angular_velocity, self.dbw_enabled) if self.dbw_enabled: self.publish(self.throttle, self.brake, self.steer) rate.sleep() def dbw_enabled_cb(self, msg): self.dbw_enabled = msg def twist_cmd_cb(self, msg): self.req_linear_velocity = msg.twist.linear.x self.req_angular_velocity = msg.twist.angular.z def current_vel_cb(self, msg): self.current_velocity = 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)
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.) min_speed = 0.0 self.dbw_en = False 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) params = {"wheel_base": wheel_base, "wheel_radius":wheel_radius, "steer_ratio": steer_ratio, "min_speed": min_speed, "max_lat_accel": max_lat_accel, "max_steer_angle": max_steer_angle, "brake_deadband": brake_deadband, "accel_limit": accel_limit, "decel_limit": decel_limit, "vehicle_mass":vehicle_mass, "fuel_capacity":fuel_capacity} # TODO: Create `TwistController` object self.controller = Controller(**params) # TODO: Subscribe to all the topics you need to rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) self.loop() def loop(self): rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): try: params = {'twist_cmd': self.twist_cmd, 'current_vel': self.current_velocity} throttle, brake, steer = self.controller.control(**params) if(self.dbw_en): self.publish(throttle, brake, steer) else: self.controller.reset() rate.sleep() except AttributeError: pass # Callbacks for subscribers def dbw_enabled_cb(self, dbw_en): self.dbw_en = dbw_en def twist_cmd_cb(self, twist): self.twist_cmd = twist def current_velocity_cb(self, vel): self.current_velocity = vel 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)
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node') # Define some vehicle params 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.) # Initialize steering, throttle and brake publishers 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 acc2torque = vehicle_mass * vehicle_mass #trasformation from acceleration 2 torque self.controller = Controller(wheel_base, steer_ratio, max_lat_accel, max_steer_angle, acc2torque, decel_limit, accel_limit, wheel_radius) # TODO: Subscribe to all the topics you need to rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) """ Service initialized: - Publishers: - steer_pub = publisher of the steering cmd - throttle_pub = publisher of the throttle cmd - brake_pub = publisher of the brake cmd - Subscribers: - twist_cmd_cb = subscribe to requested twist - current_velocity_cb = subscribe to requested velocity - dbw_enabled_cb = subscribe to driving mode """ # variables self.dbw_enabled = False self.current_velocity = None self.twist_cmd = None self.target_velocity = None 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 self.current_velocity is not None and self.twist_cmd is not None: v_limit = rospy.get_param( '/waypoint_loader/velocity' ) / 3.6 * 0.9 # Speed limit (with margin) throttle, brake, steering = self.controller.control( self.twist_cmd.linear, self.twist_cmd.angular, self.current_velocity.linear, self.current_velocity.angular, self.dbw_enabled, v_limit) # Immediatly before publish control dbw_enable state if self.dbw_enabled: print "publish" self.publish(throttle, brake, steering) # slleep until next period fire 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) 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 twist_cmd_cb(self, msg): self.twist_cmd = msg.twist def current_velocity_cb(self, msg): self.current_velocity = msg.twist #rospy.logerr(msg.twist) def dbw_enabled_cb(self, msg): self.dbw_enabled = msg.data
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) args = { '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 } self.controller = Controller(**args) # TODO: Subscribe to all the topics you need to self.current_velocity = None self.twist_cmd = None self.dbw_enabled = None rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) 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 self.current_velocity and self.twist_cmd: throttle, brake, steering = self.controller.control(self.twist_cmd, self.current_velocity, self.dbw_enabled) if self.dbw_enabled: self.publish(throttle, brake, steering) 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) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) def current_velocity_cb(self, msg): self.current_velocity = msg def twist_cmd_cb(self, msg): self.twist_cmd = msg def dbw_enabled_cb(self, msg): self.dbw_enabled = msg
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) self.controller = Controller(wheel_base, steer_ratio, max_lat_accel, max_steer_angle, vehicle_mass, wheel_radius, brake_deadband) # Subscribe to all needed topics rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb) self.dbw_enabled = False rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) self.curr_lin_vel = None self.curr_ang_vel = None self.trgt_lin_vel = None self.trgt_ang_vel = None self.loop() def loop(self): rate = rospy.Rate(CMD_RATE) while not rospy.is_shutdown(): if self.curr_lin_vel is None \ or self.curr_ang_vel is None \ or self.trgt_lin_vel is None \ or self.trgt_ang_vel is None: continue # Get predicted throttle, brake, and steering using `twist_controller` throttle, brake, steer = self.controller.control( self.trgt_lin_vel, self.trgt_ang_vel, self.curr_lin_vel, self.dbw_enabled) # You should only publish the control commands if dbw is enabled if self.dbw_enabled: self.publish(throttle, brake, steer) 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) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) # Callback function for twist_cmd def twist_cb(self, twist_stamped_msg): self.trgt_lin_vel = twist_stamped_msg.twist.linear.x self.trgt_ang_vel = twist_stamped_msg.twist.angular.z # Callback function for current_velocity def current_velocity_cb(self, twist_stamped_msg): self.curr_lin_vel = twist_stamped_msg.twist.linear.x self.curr_ang_vel = twist_stamped_msg.twist.angular.z # Callback function for current_velocity def dbw_enabled_cb(self, bool_msg): self.dbw_enabled = bool_msg.data
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node', log_level=rospy.INFO) vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) decel_limit = rospy.get_param('~decel_limit', -5) accel_limit = rospy.get_param('~accel_limit', 1.) 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) self.controller = TwistController( YawController(wheel_base, steer_ratio, MIN_SPEED, max_lat_accel, max_steer_angle), accel_limit, decel_limit, vehicle_mass) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.enabled_cb) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb) self.dbw_enabled = False self.velocity = None self.proposed_linear = None self.proposed_angular = None self.loop() def enabled_cb(self, msg): self.dbw_enabled = msg.data if DEBUG: rospy.loginfo("DBW enabled = " + str(self.dbw_enabled)) def twist_cb(self, msg): self.proposed_linear = msg.twist.linear.x self.proposed_angular = msg.twist.angular.z if DEBUG: rospy.logdebug("Twist update: angular = %d; linear = %d" % (self.proposed_angular, self.proposed_linear)) def velocity_cb(self, msg): self.velocity = msg.twist.linear.x if DEBUG: rospy.logdebug("Velocity update: %d" % self.velocity) def loop(self): rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): if self.velocity is not None \ and self.proposed_linear is not None \ and self.proposed_angular is not None: throttle, brake, steer = self.controller.control( self.proposed_linear, self.proposed_angular, self.velocity, self.dbw_enabled) if self.dbw_enabled: self.publish(throttle, brake, steer) rate.sleep() def publish(self, throttle, brake, steer): if DEBUG: rospy.loginfo("Controls: %.15f:%.15f:%.15f" % (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)
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.is_dbw_enabled = False self.last_twist_command = None self.current_velocity = None self.current_pose = None self.final_waypoints = None self.previous_loop_time = rospy.get_rostime() self.previous_debug_time = rospy.get_rostime() self.throttle_pid = pid.PID(kp=10.0, ki=0.01, kd=1.0, mn=decel_limit, mx=0.5 * accel_limit) self.brake_pid = pid.PID(kp=120.0, ki=0.0, kd=1.0, mn=brake_deadband, mx=5000) self.steering_pid = pid.PID(kp=1.0, ki=0.1, kd=0.6, mn=-max_steer_angle, mx=max_steer_angle) 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.throttle_pid, self.brake_pid, self.steering_pid) # TODO: Subscribe to all the topics you need to rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_commands_cb, queue_size=1) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb, queue_size=1) rospy.Subscriber('/current_pose', geometry_msgs.msg.PoseStamped, self.current_pose_cb, queue_size=1) rospy.Subscriber('/final_waypoints', styx_msgs.msg.Lane, self.final_wp_cb, queue_size=1) 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 # throttle, brake, steering = self.controller.control(<proposed linear velocity>, # <proposed angular velocity>, # <current linear velocity>, # <dbw status>, # <any other argument you need>) # if <dbw is enabled>: # self.publish(throttle, brake, steer) data = [ self.last_twist_command, self.current_velocity, self.current_pose, self.final_waypoints ] data_ready = all([x is not None for x in data]) if self.is_dbw_enabled and data_ready: current_time = rospy.get_rostime() ros_duration = current_time - self.previous_loop_time duration_in_seconds = ros_duration.secs + (1e-9 * ros_duration.nsecs) self.previous_loop_time = current_time # Linear velocity and cross track error calculations based on difference between # current Vs desired speed after x waypoints linear_velocity_error = self.final_waypoints[ 1].twist.twist.linear.x - self.current_velocity.linear.x cte = dbw_support.get_cross_track_error( self.final_waypoints, self.current_pose) # Node output calculations throttle, brake, steering = self.controller.control( linear_velocity_error, cte, duration_in_seconds) # Publish self.publish(throttle, brake, steering) 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) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) def current_velocity_cb(self, msg): self.current_velocity = msg.twist def current_pose_cb(self, msg): self.current_pose = msg.pose def twist_commands_cb(self, msg): self.last_twist_command = msg.twist def dbw_enabled_cb(self, msg): self.is_dbw_enabled = bool(msg.data) if self.is_dbw_enabled is True: self.throttle_pid.reset() self.brake_pid.reset() self.steering_pid.reset() def final_wp_cb(self, msg): self.final_waypoints = msg.waypoints
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.) throttle_gains = Gains(rospy.get_param('~throttle_Kp', 0.0), rospy.get_param('~throttle_Ki', 0.0), rospy.get_param('~throttle_Kd', 0.0)) steering_gains = Gains(rospy.get_param('~steering_Kp', 0.0), rospy.get_param('~steering_Ki', 0.0), rospy.get_param('~steering_Kd', 0.0)) # Parameterize loop rate with 50 Hz as default if not found self.rate_param = rospy.get_param('~rate_param', 50) # Publishers 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: Pass params to `Controller` constructor self.controller = Controller(wheel_base=wheel_base, steer_ratio=steer_ratio, min_speed=1.0 * 0.447, max_lat_accel=max_lat_accel, max_steer_angle=max_steer_angle, throttle_gains=throttle_gains, steering_gains=steering_gains, accel_limit=accel_limit, decel_limit=decel_limit) # Subscriptions rospy.Subscriber('/dbw_enabled', Bool, self.dbw_enabled_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb, queue_size=1) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb, queue_size=1) # Member vars self.dbw_enabled = True self.current_velocity = None self.twist_cmd = None self.loop() def loop(self): rate = rospy.Rate( self.rate_param ) # 50Hz means that it goes through the loop 50 times per sec 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 self.twist_cmd is None or self.current_velocity is None: continue throttle, brake, steering = self.controller.control( self.twist_cmd.twist.linear, self.twist_cmd.twist.angular, self.current_velocity.twist.linear, self.dbw_enabled) if self.dbw_enabled: self.publish(throttle, brake, steering) rate.sleep() def publish(self, throttle, brake, steer): rospy.loginfo("publishing throttle %f, brake %f, steer %f", throttle, brake, steer) # We'll either publish the throttle or brake, not both if throttle != 0: tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle self.throttle_pub.publish(tcmd) else: bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) scmd = SteeringCmd() scmd.enable = True scmd.steering_wheel_angle_cmd = steer self.steer_pub.publish(scmd) def dbw_enabled_cb(self, msg): rospy.loginfo("DBW status changed to: %s", msg) self.dbw_enabled = msg def current_velocity_cb(self, msg): self.current_velocity = msg def twist_cmd_cb(self, msg): rospy.loginfo("Received twist command %s", msg) self.twist_cmd = msg
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.) # Controller # numbers are invented controller_params = { 'kp': 1.5, 'ki': 0.003, 'kd': 3.5, 'wheel_base': wheel_base, 'wheel_radius': wheel_radius, 'steer_ratio': steer_ratio, 'min_speed': 0.1, 'max_lat_accel': max_lat_accel, 'max_steer_angle': max_steer_angle, 'tau': 0.5, 'ts': 0.02, 'vehicle_mass': vehicle_mass } self.controller = Controller(**controller_params) # properties self.dbw_enabled = False self.last_velocity = None self.twist_cmd = None # Publishers 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) # Subscribers 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) # run the main loop self.loop() def dbw_enabled_cb(self, msg): self.dbw_enabled = msg.data def current_velocity_cb(self, msg): self.last_velocity = msg.twist def twist_cmd_cb(self, msg): self.twist_cmd = msg.twist def loop(self): rospy.wait_for_message('/vehicle/dbw_enabled', Bool) rospy.wait_for_message('/current_velocity', TwistStamped) rospy.wait_for_message('/twist_cmd', TwistStamped) rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): if self.dbw_enabled: throttle, brake, steer = self.controller.control( self.twist_cmd, self.last_velocity) self.publish(throttle, brake, steer) else: self.controller.reset() 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) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd)
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.) throttle_gains = Gains(rospy.get_param('~throttle_Kp', 0.0), rospy.get_param('~throttle_Ki', 0.0), rospy.get_param('~throttle_Kd', 0.0)) steering_gains = Gains(rospy.get_param('~steering_Kp', 0.0), rospy.get_param('~steering_Ki', 0.0), rospy.get_param('~steering_Kd', 0.0)) # Parameterize loop rate with 50 Hz as default if not found self.rate_param = rospy.get_param('~rate_param', 50) # Publishers 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: Pass params to `Controller` constructor self.controller = Controller(wheel_base=wheel_base, steer_ratio=steer_ratio, min_speed=1.0 * 0.447, max_lat_accel=max_lat_accel, max_steer_angle=max_steer_angle, throttle_gains=throttle_gains, steering_gains=steering_gains, accel_limit=accel_limit, decel_limit=decel_limit) # Subscriptions rospy.Subscriber('/dbw_enabled', Bool, self.dbw_enabled_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb, queue_size=1) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb, queue_size=1) # Member vars self.dbw_enabled = True self.current_velocity = None self.twist_cmd = None self.loop()
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 = self.steering = 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 # throttle, brake, steering = self.controller.control(<proposed linear velocity>, # <proposed angular velocity>, # <current linear velocity>, # <dbw status>, # <any other argument you need>) 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 <dbw is enabled>: # self.publish(throttle, brake, steer) 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)
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node') info = CarInfo() info.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) info.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) info.brake_deadband = rospy.get_param('~brake_deadband', .1) info.decel_limit = rospy.get_param('~decel_limit', -5) info.accel_limit = rospy.get_param('~accel_limit', 1.) info.wheel_radius = rospy.get_param('~wheel_radius', 0.2413) info.wheel_base = rospy.get_param('~wheel_base', 2.8498) info.steer_ratio = rospy.get_param('~steer_ratio', 14.8) info.max_lat_accel = rospy.get_param('~max_lat_accel', 3.) info.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) self.current_velocity = None self.twist_cmd = None self.dbw_enabled = True self.previous_timestamp = rospy.Time.now().to_sec() self.reset = True # Create `TwistController` object # self.controller = TwistController(<Arguments you wish to provide>) self.controller = Controller(info) # Subscribe to all the topics you need to rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb, queue_size=1) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb, queue_size=1) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1) self.loop() def velocity_cb(self, msg): self.current_velocity = msg def twist_cb(self, msg): self.twist_cmd = msg def dbw_enabled_cb(self, msg): try: self.dbw_enabled = bool(msg.data) except Exception: self.dbw_enabled = msg def loop(self): rate = rospy.Rate(10) # 10Hz while not rospy.is_shutdown(): # Get time delta current_timestamp = rospy.get_time() delta_time = current_timestamp - self.previous_timestamp self.previous_timestamp = current_timestamp # 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>, # <proposed angular velocity>, # <current linear velocity>, # <dbw status>, # <any other argument you need>) # if <dbw is enabled>: # self.publish(throttle, brake, steer) if self.dbw_enabled and self.current_velocity is not None and self.twist_cmd is not None: if self.reset: self.controller.reset() self.reset = False throttle, brake, steer = self.controller.control( twist_cmd=self.twist_cmd, current_velocity=self.current_velocity, delta_time=delta_time) rospy.loginfo("CURRENT: %.3f, TARGET: %.3f, BRAKE: %.3f", self.current_velocity.twist.linear.x, self.twist_cmd.twist.linear.x, brake) # if brake > 0.0: # rospy.loginfo("THROTTLE: %s, BRAKE: %s", throttle, brake) self.publish(throttle, brake, steer) else: self.reset = True 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) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd)
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node') # Save param from launch file as dictionary ros_param_dict = { 'vehicle_mass': rospy.get_param('~vehicle_mass', 1736.35), # must need for stoping the car 'fuel_capacity': rospy.get_param('~fuel_capacity', 13.5), # optional go precise with the weight '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), # must '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.) } # Subscribe to all the topics you need to rospy.Subscriber('/current_velocity', TwistStamped, self.cur_velocity_cb) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) # Publish topics. 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) self.controller = Controller(**ros_param_dict) # List of variables need to use self.dbw_enabled = None self.cur_linear_velocity = None # may need low pass filter to reduce noisy self.cur_angular_velocity = None self.target_linear_velocity = None self.target_angular_velocity = None # flag for checking msg received self.twist_cmd = False self.current_velocity = False self.loop() def cur_velocity_cb(self, msg): self.current_velocity = True self.cur_linear_velocity = msg.twist.linear.x self.cur_angular_velocity = msg.twist.angular.z def twist_cmd_cb(self, msg): self.twist_cmd = True self.target_linear_velocity = msg.twist.linear.x self.target_angular_velocity = msg.twist.angular.z def dbw_enabled_cb(self, msg): # painful lesson: uncheck the manual to activate the topic otherwise nothing in echo topic self.dbw_enabled = msg.data rospy.loginfo(rospy.get_caller_id() + " dbw status is : %s", self.dbw_enabled) def loop(self): """ Get predicted throttle, brake, and steering using `twist_controller` throttle, brake, steering = self.controller.control(<proposed linear velocity>, <proposed angular velocity>, <current linear velocity>, <dbw status>, <any other argument you need>) """ rate = rospy.Rate(50) # 50Hz Do not Change! Lower than 20Hz will shut down the vehicle while not rospy.is_shutdown(): # When current_velocity or twist_cmd is not ready if ((not self.current_velocity) or (not self.twist_cmd)): continue throttle, brake, steer = self.controller.control(self.target_linear_velocity, self.target_angular_velocity, self.cur_linear_velocity, self.dbw_enabled) # You should only publish the control commands if dbw is enabled if self.dbw_enabled: self.publish(throttle, brake, steer) else: self.controller.reset() 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) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake 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.) max_throttle = rospy.get_param('~max_throttle', 0.3) max_brake = rospy.get_param('~max_brake', -0.3) min_speed = 0.0 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 <Arguments you wish to provide> self.controller = Controller(decel_limit, accel_limit, max_steer_angle, max_lat_accel, min_speed, wheel_base, steer_ratio, vehicle_mass, wheel_radius, max_throttle, max_brake) # self.yaw_controller = YawController(wheel_base, steer_ratio, 0.0, max_lat_accel, max_steer_angle) self.dbw_enabled = None self.current_velocity = None self.twist_cmd = None self.pose = None self.final_waypoints = None self.prev_clk = rospy.get_rostime().nsecs self.prev_clk_ready = False # TODO: Subscribe to all the topics you need to rospy.Subscriber('/current_velocity', TwistStamped, self.curr_vel_cb, queue_size=1) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb, queue_size=1) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1) rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb, queue_size=1) rospy.Subscriber('/final_waypoints', Lane, self.final_waypoints_cb, queue_size=1) self.loop() def required_all(self): required = [ self.dbw_enabled, self.current_velocity, self.twist_cmd, self.pose, self.final_waypoints ] return all([p is not None for p in required]) def curr_vel_cb(self, curr_vel_msg): # rospy.loginfo("current_velocity = {}".format(curr_vel_msg.twist)) self.current_velocity = curr_vel_msg.twist def twist_cmd_cb(self, twist_cmd_msg): # rospy.loginfo("twist_cmd = {}".format(twist_cmd_msg.twist)) self.twist_cmd = twist_cmd_msg.twist def dbw_enabled_cb(self, dbw_enabled): # rospy.loginfo("dbw_enabled = {}".format(dbw_enabled.data)) self.dbw_enabled = dbw_enabled.data def final_waypoints_cb(self, final_waypoints): self.final_waypoints = final_waypoints.waypoints def pose_cb(self, pose): # rospy.loginfo("pose = {}".format(pose.pose)) self.pose = pose # rospy.loginfo("pose x, y, yaw = {}, {}, {}".format(self.pose.position.x, # self.pose.position.y, helper.yaw_from_orientation(self.pose.orientation))) 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 self.required_all(): rospy.loginfo('Waiting for all params ...') continue # TODO: Move clock to pid controllers clk = rospy.get_rostime() if not self.prev_clk_ready: self.prev_clk_ready = True continue # Yes, I now it will be always 0.02 but in case someone will change rate ... delta_t = (clk.nsecs - self.prev_clk) * 1e-9 # rospy.loginfo('delta_t = {}'.format(delta_t)) self.prev_clk = clk.nsecs target_linear_velocity = abs( self.twist_cmd.linear.x ) # abs - fixing strange bug in twist_cmd ... target_angular_velocity = self.twist_cmd.angular.z current_linear_velocity = self.current_velocity.linear.x current_angular_velocity = self.current_velocity.angular.z current_yaw = helper.yaw_from_orientation( self.pose.pose.orientation) rospy.loginfo( "tlv = {}, clv = {}, tav = {}, cav = {}, cyaw = {}".format( target_linear_velocity, current_linear_velocity, target_angular_velocity, current_angular_velocity, current_yaw)) # rospy.loginfo('current_yaw = {}'.format(current_yaw)) steer_cte = helper.calc_steer_cte(self.pose, self.final_waypoints) rospy.loginfo('STEER_CTE = {}'.format(steer_cte)) throttle, brake, steering = self.controller.control( target_linear_velocity, current_linear_velocity, target_angular_velocity, current_angular_velocity, steer_cte, self.dbw_enabled) # <proposed linear velocity>, # <proposed angular velocity>, # <current linear velocity>, # <dbw status>, # <any other argument you need>) # steering1 = self.yaw_controller.get_steering(target_linear_velocity, target_angular_velocity, current_linear_velocity) rospy.loginfo("throttle, brake, steering = {}, {}, {}".format( throttle, brake, steering)) # rospy.loginfo("steering1 = {}".format(steering1)) if self.dbw_enabled: self.publish(throttle, brake, steering) 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) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE # BrakeCmd.CMD_TORQUE, BrakeCmd.CMD_PERCENT bcmd.pedal_cmd = brake 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.) decel_limit = rospy.get_param('~max_lon_accel', 3.) 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) # NOTE: To prevent Carla from moving requires about 700 Nm of torque # (can be calculated from the `vehicle_mass`, `wheel_radius`, and desired # acceleration) # Create the `Controller` object self.controller = Controller(wheel_base, wheel_radius, steer_ratio, max_lat_accel, max_steer_angle, decel_limit, vehicle_mass) # Subscribe to all the necessary topics self.dbw_enabled_sub = rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) self.curr_velocity_sub = rospy.Subscriber('/current_velocity', TwistStamped, self.curr_velocity_cb) self.twist_cmd_sub = rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb) self.dbw_enabled = None # From '/vehicle/dbw_enabled' self.curr_velocity = None # From '/current_velocity' self.linear_vel = None # From '/twist_cmd' self.angular_vel = None # From '/twist_cmd' self.loop() def loop(self): rate = rospy.Rate(50) # 50Hz # NOTE: if the rate is below 10Hz, Carla will disengage (reverting control # to the driver) # Better ensure that the control commands are published at 50Hz while not rospy.is_shutdown(): # You should only publish the control commands if dbw is enabled throttle, brake, steer = self.controller.control( self.curr_velocity, self.dbw_enabled, self.linear_vel, self.angular_vel, ) if self.dbw_enabled: self.publish(throttle, brake, steer) 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) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) def dbw_enabled_cb(self, msg): self.dbw_enabled = msg def curr_velocity_cb(self, msg): self.curr_velocity = msg.twist.linear.x def twist_cb(self, msg): self.linear_vel = msg.twist.linear.x self.angular_vel = msg.twist.angular.z
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.) max_throttle = rospy.get_param('~max_throttle', 0.3) max_brake = rospy.get_param('~max_brake', -0.3) min_speed = 0.0 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 <Arguments you wish to provide> self.controller = Controller(decel_limit, accel_limit, max_steer_angle, max_lat_accel, min_speed, wheel_base, steer_ratio, vehicle_mass, wheel_radius, max_throttle, max_brake) # self.yaw_controller = YawController(wheel_base, steer_ratio, 0.0, max_lat_accel, max_steer_angle) self.dbw_enabled = None self.current_velocity = None self.twist_cmd = None self.pose = None self.final_waypoints = None self.prev_clk = rospy.get_rostime().nsecs self.prev_clk_ready = False # TODO: Subscribe to all the topics you need to rospy.Subscriber('/current_velocity', TwistStamped, self.curr_vel_cb, queue_size=1) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb, queue_size=1) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1) rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb, queue_size=1) rospy.Subscriber('/final_waypoints', Lane, self.final_waypoints_cb, queue_size=1) self.loop()
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) self.controller = Controller(vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle) 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(): 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 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 dbw_enabled_cb(self, dbw_enabled): self.dbw_enabled = dbw_enabled def velocity_cb(self, velocity): self.current_vel = velocity.twist.linear.x def twist_cb(self, twist): self.linear_vel = twist.twist.linear.x self.angular_vel = twist.twist.angular.z
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) args = [vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle] self.controller = Controller(*args) # Subscriptions rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb, queue_size=1) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb, queue_size=1) rospy.Subscriber('/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1) # local variables self.dbw_enabled = True self.current_velocity = None self.twist_cmd = None self.loop() def loop(self): rate = rospy.Rate(COMMAND_FREQUENCY) while not rospy.is_shutdown(): # Will ignore when we don't have info from `current_velocity`, `twist_cmd` and when dbw is not enabled if (self.twist_cmd is None) or (self.current_velocity is None) or (not self.dbw_enabled): continue throttle, brake, steering = self.controller.control(self.twist_cmd.twist.linear, self.twist_cmd.twist.angular, self.current_velocity.twist.linear) self.publish(throttle, brake, steering) 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) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) # Helper for subscriber below. def twist_cmd_cb(self, msg): """Subscribe to twist_cmd.""" self.twist_cmd = msg def current_velocity_cb(self, msg): """Subscribe to current_velocity_cb.""" self.current_velocity = msg def dbw_enabled_cb(self, msg): """Subscribe to dbw_enabled.""" self.dbw_enabled = msg