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(<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) 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 <dbw is enabled>: # self.publish(throttle, brake, steer) if not None in (self.current_vel, self.linear_vel, self.angular_vel): self.throttle, self.brake, self.steering = self.controller.control( self.dbw_enabled, self.current_vel, 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.) max_throttle = rospy.get_param('~max_throttle', 1.) 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 `TwistController` object self.controller = Controller(accel_limit=accel_limit, decel_limit=decel_limit, wheel_base=wheel_base, steer_ratio=steer_ratio, max_lat_accel=max_lat_accel, max_steer_angle=max_steer_angle, vehicle_mass=vehicle_mass, fuel_capacity=fuel_capacity, wheel_radius=wheel_radius, brake_deadband=brake_deadband, max_throttle=max_throttle) self.dbw_enabled = False self.twist = None self.current_velocity = None self.last_dbw_status = False self.last_time = None self.last_action = '' # Subscribe to all the topics you need to rospy.Subscriber('/current_velocity', TwistStamped, self.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(): # Get predicted throttle, brake, and steering using `twist_controller` # You should only publish the control commands if dbw is enabled now = rospy.get_rostime() if self.dbw_enabled and self.current_velocity is not None \ and self.twist is not None \ and self.last_time: self.reset_controller_if_needed() diff = now - self.last_time throttle, brake, steer = self.controller.control( self.current_velocity, self.twist, diff.to_sec()) self.publish(throttle, brake, steer) # Update variables self.last_time = now self.last_dbw_status = self.dbw_enabled rate.sleep() def reset_controller_if_needed(self): if self.dbw_enabled != self.last_dbw_status and self.dbw_enabled: self.controller.reset() def publish(self, throttle, brake, steer): tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle 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 # do not publish throttle and brake at the same time, # unless the action is different action = 'brake' if brake > 0.0 else 'throttle' if action != self.last_action: self.brake_pub.publish(bcmd) self.throttle_pub.publish(tcmd) elif action == 'brake': self.brake_pub.publish(bcmd) elif action == 'throttle': self.throttle_pub.publish(tcmd) self.last_action = action def velocity_cb(self, msg): self.current_velocity = msg.twist.linear.x def twist_cmd_cb(self, msg): self.twist = msg.twist def dbw_enabled_cb(self, msg): self.dbw_enabled = msg.data rospy.logwarn('dbw_enabled: %s', self.dbw_enabled)
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node') rospy.loginfo('Start initialization of DBWNode') 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.linear_velocity_future = None self.angular_velocity_future = None self.linear_velocity_current = None self.angular_velocity_current = None self.acceleration_current = None self.dbw_enabled = None self.time_help = None self.rate = 50 # Rate in Hz 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(vehicle_mass=vehicle_mass, fuel_capacity=fuel_capacity, brake_deadband=brake_deadband, decel_limit=decel_limit, accel_limit=accel_limit, wheel_radius=wheel_radius, rate=self.rate) # TODO: Subscribe to all the topics you need to rospy.Subscriber('/twist_cmd', TwistStamped, self.twistcmd_cb, queue_size=1) rospy.Subscriber('/current_velocity', TwistStamped, self.cur_vel_cb, queue_size=1) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1) rospy.loginfo('DBWNode: Subscribed to relevant topics') self.loop() def loop(self): rospy.loginfo('DBWNode: Started looping') rate = rospy.Rate(self.rate) while not rospy.is_shutdown(): now = rospy.get_rostime() if self.has_valid_data(): diff = now - self.time_help throttle, brake, steer = self.controller.control( linear_velocity_future=self.linear_velocity_future, angular_velocity_future=self.angular_velocity_future, linear_velocity_current=self.linear_velocity_current, angular_velocity_current=self.angular_velocity_current, acceleration_current=self.acceleration_current, time_step=diff.to_sec()) #rospy.loginfo('DBWNode: Controller output: throttle -> %.2f brake -> %.2f steer -> %.2f', throttle, brake, steer) #rospy.loginfo('_____________________') #rospy.loginfo(self.linear_velocity_future) #rospy.loginfo(self.linear_velocity_current) self.publish(throttle, brake, steer) self.time_help = now rate.sleep() def publish(self, throttle, brake, steer): if brake > 0.0: bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) else: 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) # TODO: Implement def twistcmd_cb(self, msg): self.linear_velocity_future = msg.twist.linear.x self.angular_velocity_future = msg.twist.angular.z #rospy.loginfo('DBWNode: Updated twist') # TODO: Implement def cur_vel_cb(self, msg): if (self.linear_velocity_current is not None): self.acceleration_current = self.rate * ( self.linear_velocity_current - msg.twist.linear.x) self.linear_velocity_current = msg.twist.linear.x self.angular_velocity_current = msg.twist.angular.z #rospy.loginfo('DBWNode: Updated velocity') # TODO: Implement def dbw_enabled_cb(self, msg): self.dbw_enabled = msg.data rospy.loginfo('DBWNode: Updated dbw_enabled with %d', self.dbw_enabled) if (not self.dbw_enabled): self.controller.reset() def has_valid_data(self): return (self.time_help is not None) & (self.linear_velocity_future is not None) & ( self.linear_velocity_current is not None)
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.) loop_freq = rospy.get_param('~loop_freq', 2) self.dt = 1. / loop_freq self.dbw_enabled = False self.current_velocity = None self.desired_velocity = None 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, sample_period=self.dt) # 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, queue_size=3) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) self.loop() def twist_cmd_cb(self, msg): self.desired_velocity = msg.twist def current_velocity_cb(self, msg): self.current_velocity = msg.twist def dbw_enabled_cb(self, msg): self.dbw_enabled = msg.data self.controller.velocity_controller.reset() 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.dbw_enabled: if self.desired_velocity and self.current_velocity: throttle, brake, steering = self.controller.control( self.desired_velocity, self.current_velocity, self.dt) self.publish(throttle, brake, steering) self.desired_velocity = None 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.) parameters = { '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.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.dbw_enabled = False self.current_velocity = None self.current_setpoint = None # TODO: Create `Controller` object # self.controller = Controller(<Arguments you wish to provide>) self.controller = Controller(**parameters) # TODO: Subscribe to all the topics you need to self.current_velocity_sub = rospy.Subscriber('/current_velocity', TwistStamped, self.current_speed_cb) self.twist_cmd_sub = rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb) self.dbw_enabled_sub = rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) self.loop() def current_speed_cb(self, current_velocity): self.current_velocity = current_velocity.twist pass def twist_cmd_cb(self, twist_cmd): self.current_setpoint = twist_cmd.twist pass def dbw_enabled_cb(self, msg): rospy.logwarn('dbw_enabled %s', msg.data) self.dbw_enabled = bool(msg.data) pass 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_enabled: throttle, brake, steer = self.controller.control(self.current_setpoint.linear.x, self.current_setpoint.angular.z, self.current_velocity.linear.x, 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)
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
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node',log_level=rospy.DEBUG) 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>) min_speed = 0 throttle_pid = PID(1, 1, 1, mn=0, mx=1) brake_pid = PID(600, 10, 0, mn=0, mx=3000) steering_pid = PID(4, 1, 2, mn=-math.pi/3, mx=math.pi/3) yaw_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) low_pass_filter = LowPassFilter(1, 1) self.controller = Controller(throttle_controller = throttle_pid, brake_controller = brake_pid, steering_controller = yaw_controller, steering_adjustment_controller = steering_pid, smoothing_filter = low_pass_filter) # 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) # init valuables self.dbw_enabled = False self.proposed_velocity = (0, 0) self.current_velocity = (0, 0) self.current_position = (0, 0) # Kick off the loop operation self.loop() def loop(self): rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): # 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==True): throttle, brake, steering = self.controller.control( sample_time = 0.02, proposed_velocity = self.proposed_velocity, current_velocity = self.current_velocity) rospy.loginfo("=== lazzzy === : throttle: %f, brake: %f, steering: %f",throttle, brake, steering) 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_cb(self, msg): self.proposed_velocity = (msg.twist.linear.x, msg.twist.angular.z) self.controller.reset() def current_velocity_cb(self, msg): self.current_velocity = (msg.twist.linear.x, msg.twist.angular.z) def dbw_enabled_cb(self, msg): self.dbw_enabled = msg.data if self.dbw_enabled == False: self.controller.reset_on_dbw_enabled()
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 min_speed = 0 #?! self.controller = Controller(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius) # TODO: Subscribe to all the topics you need to rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb) self.proposed_angular_velocity = self.proposed_linear_velocity = None self.current_angular_velocity = self.current_linear_velocity = None self.dbw_enabled = False self.loop() def twist_cb(self, msg): self.proposed_angular_velocity = msg.twist.angular self.proposed_linear_velocity = msg.twist.linear return def dbw_enabled_cb(self, msg): self.dbw_enabled = msg rospy.logwarn('dbw_enabled: ' + str(self.dbw_enabled)) return def current_velocity_cb(self, msg): self.current_angular_velocity = msg.twist.angular self.current_linear_velocity = msg.twist.linear return 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_enabled: # self.publish(throttle, brake, steer) if not self.proposed_linear_velocity or not self.proposed_angular_velocity or not self.current_linear_velocity: rospy.logwarn('velocity info not available') else: throttle, brake, steering = self.controller.control( self.proposed_linear_velocity, self.proposed_angular_velocity, self.current_linear_velocity, (1. / 50), #sample time (1/50Hz) sec 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)
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) # 'brake' is a torque accel_limit = rospy.get_param('~accel_limit', 1.) # 'accel' is a percentage 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', 0.1) Kp = rospy.get_param('~pid_kp', 1.1) Ki = rospy.get_param('~pid_ki', 0.010) Kd = rospy.get_param('~pid_kd', 0.005) pid_cmd_range = rospy.get_param('~pid_cmd_range', 4) filter_tau = rospy.get_param('~filter_tau', 0.0) 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.steer_pub = rospy.Publisher('/vehicle/steering_cmd', SteeringCmd, queue_size=1) self.vx_pub = rospy.Publisher('/debug_vx', Float64, queue_size=1) self.sz_pub = rospy.Publisher('/debug_sz', Float64, queue_size=1) self.vxd_pub = rospy.Publisher('/debug_vxd', Float64, queue_size=1) self.szd_pub = rospy.Publisher('/debug_szd', Float64, queue_size=1) # def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle): self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) # def __init__(self, tau, ts): self.filter = LowPassFilter(filter_tau, 0.8) # hm why 0.8? # Pid controller for the target velocity (decel_limit is already negative) self.pid_vel = PID(Kp, Ki, Kd, decel_limit, accel_limit) # write controller self.controller = Controller(self.yaw_controller, self.pid_vel, self.filter) # set controller parameters vehicle_mass_offset = 25.0 + 70.0 + 30.0 # additional weight (gas, passenger, load) self.controller.set_vehicle_parameters(vehicle_mass, vehicle_mass_offset, brake_deadband, wheel_radius) self.current_velocity = None self.target_velocity = None self.dbw_enabled = True # maybe change to False by default self.pose = None rospy.Subscriber("/current_velocity", TwistStamped, self.velocity_cb) rospy.Subscriber("/twist_cmd", TwistStamped, self.twist_cb) rospy.Subscriber("/vehicle/dbw_enabled", Bool, self.dbw_cb) rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb) self.loop() def velocity_cb(self, msg): self.current_velocity = msg.twist self.vx_pub.publish(self.current_velocity.linear.x) self.sz_pub.publish(self.current_velocity.angular.z) def twist_cb(self, msg): self.target_velocity = msg.twist self.vxd_pub.publish(self.target_velocity.linear.x) self.szd_pub.publish(self.target_velocity.angular.z) def dbw_cb(self, msg): self.dbw_enabled = msg.data rospy.loginfo('received dbw_enabled: %s', str(msg.data)) def pose_cb(self, msg): self.pose = msg.pose 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 self.current_velocity: rospy.logwarn('no current velocity has been set') rate.sleep() continue if not self.target_velocity: rospy.logwarn('no target velocity has been set') rate.sleep() continue if not self.pose: rospy.logwarn('no pose has been set') rate.sleep() continue if not self.dbw_enabled: rospy.logdebug('no driving by wire') #Reset the PID controller self.controller.reset() rate.sleep() continue throttle, brake, steer = self.controller.control( current_velocity=self.current_velocity.linear.x, linear_velocity=self.target_velocity.linear.x, angular_velocity=self.target_velocity.angular.z) 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)
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.) #I added min_speed here extra to video but hardcoded else where so I put it here as a variable to be reset if needed min_speed = 0.1 # 10 #0.1 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>) # i missed this got error evenutally about controller # all atribute from above ''' self.controller = Controller( vehicle_mass = vehicle_mass , # i thkn i am not using kwargs correctly https://pythontips.com/2013/08/04/args-and-kwargs-in-python-explained/ 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, min_speed = min_speed)# I added min_speed here was hardcoced in video as 0.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', min_speed = 'min_speed') ''' #another attempt around **kwargs##belwo seems to work 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, min_speed) # TODO: Subscribe to all the topics you need to #from Q+A rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) #turning off for a test rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb) self.current_vel = None self.current_ang_vel = None self.dbw_enabled = False # False # None # should this be false? self.linear_vel = None self.angular_vel = None self.throttle = self.steering = self.brake = 0 self.loop() def loop(self): rate = rospy.Rate( 50 ) # maybe small difference at 10# 20 made no apparent difference #original value 50 I am trying to fix the passing the green dots problem by adjusting this rate# 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) #self.throttle = 1# testing to see it is executing in here and it is #self.brake = 0 #self.steering = 0.5#this much worked..test #throttle, brake, steering = self.controller.control(<proposed linear velocity>, # <proposed angular velocity>, # <current linear velocity>, # <dbw status>, # <any other argument you need>) #self.dbw_enabled = False #True# a test It does seem to go in here and execute if self.dbw_enabled: # changed== True : #was this in video <dbw is enabled>: self.publish(self.throttle, self.brake, self.steering) #self.publish(1,0,0)#this much worked..test rate.sleep() def dbw_enabled_cb(self, msg): #from Q+A self.dbw_enabled = msg def twist_cb(self, msg): # from Q+A self.linear_vel = msg.twist.linear.x self.angular_vel = msg.twist.angular.z # had a typo here def velocity_cb(self, msg): #from Q+A 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.) #Minimum speed to steer steering_min_speed = 2.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) #initialize freq, dbw, target & current twist values self.sample_freq = 50 self.is_dbw_enabled = False self.target_twist = None self.current_twist = None # TODO: Create `Controller` object # self.controller = Controller(<Arguments you wish to provide>) self.controller = Controller(wheel_base, steer_ratio, steering_min_speed, max_lat_accel, max_steer_angle, brake_deadband, accel_limit, decel_limit, vehicle_mass, wheel_radius, fuel_capacity, self.sample_freq) # TODO: Subscribe to all the topics you need to rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.current_twist_cb) rospy.Subscriber('/twist_cmd', TwistStamped, self.target_twist_cb) ''' #Debug comment out later rospy.Subscriber('/vehicle/steering_report', SteeringReport, self.steer_cb) rospy.Subscriber('/vehicle/throttle_report', ThrottleReport, self.throttle_cb) rospy.Subscriber('/vehicle/brake_report', BrakeReport, self.brake_cb) rospy.Subscriber('/vehicle/steering_cmd', SteeringCmd, self.steer_cb) rospy.Subscriber('/vehicle/throttle_cmd', ThrottleCmd, self.throttle_cb) rospy.Subscriber('/vehicle/brake_cmd', BrakeCmd, self.brake_cb) ''' self.loop() def loop(self): rate = rospy.Rate(self.sample_freq) # 50Hz while not rospy.is_shutdown(): if not self.target_twist or not self.current_twist: continue # TODO: Get predicted throttle, brake, and steering using `twist_controller` # You should only publish the control commands if dbw is enabled if self.is_dbw_enabled: self.throttle, self.brake, self.steering = self.controller.control( self.is_dbw_enabled, self.target_twist.linear.x, self.target_twist.angular.z, self.current_twist.linear.x) self.publish(self.throttle, self.brake, self.steering) rate.sleep() def target_twist_cb(self, msg): rospy.loginfo("target twist callback") self.target_twist = msg.twist def current_twist_cb(self, msg): rospy.loginfo("current twist callback") self.current_twist = msg.twist def dbw_enabled_cb(self, msg): self.is_dbw_enabled = msg.data rospy.loginfo("dbw_enabled callback invoked with value %s", self.is_dbw_enabled) #Debug log invocation ''' def steer_cb(self, msg): rospy.logwarn("ROSBAG throttle is %s", msg.pedal_cmd) def throttle_cb(self, msg): rospy.logwarn("ROSBAG throttle is %s", msg.pedal_cmd) def brake_cb(self, msg): rospy.logwarn("ROSBAG brake is %s", msg.pedal_cmd) ''' 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') self.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) self.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) self.brake_deadband = rospy.get_param('~brake_deadband', .1) self.decel_limit = rospy.get_param('~decel_limit', -5) self.accel_limit = rospy.get_param('~accel_limit', 1.) self.wheel_radius = rospy.get_param('~wheel_radius', 0.2413) self.wheel_base = rospy.get_param('~wheel_base', 2.8498) self.steer_ratio = rospy.get_param('~steer_ratio', 14.8) self.max_lat_accel = rospy.get_param('~max_lat_accel', 3.) self.max_steer_angle = rospy.get_param('~max_steer_angle', 8.) self.min_speed = 0.1 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_pose = None # needed? self.dbw_enabled = True self.reset_flag = True self.current_velocity = None self.twist_cmd = None # TIME self.previous_timestamp = rospy.get_time() # Create `Controller` object self.controller = Controller(self.wheel_base, self.steer_ratio, self.min_speed, self.max_lat_accel, self.max_steer_angle, self.decel_limit, self.accel_limit, self.brake_deadband, self.vehicle_mass, self.fuel_capacity, self.wheel_radius) # Subscribe to all the topics you need to rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_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) self.loop() def loop(self): rate = rospy.Rate(50) #Hz while not rospy.is_shutdown(): # TIME current_timestamp = rospy.get_time() del_time = current_timestamp - self.previous_timestamp self.previous_timestamp = current_timestamp if self.dbw_enabled and self.current_velocity is not None and self.twist_cmd is not None: if self.reset_flag: self.controller.reset() self.reset_flag = False throttle, brake, steering = self.controller.control( self.twist_cmd, self.current_velocity, del_time) self.publish(throttle, brake, steering) else: self.reset_flag = True rate.sleep() def dbw_enabled_cb(self, dbw_enabled): try: self.dbw_enabled = bool(dbw_enabled.data) except Exception: self.dbw_enabled = dbw_enabled def current_velocity_cb(self, current_velocity): self.current_velocity = current_velocity def twist_cmd_cb(self, twist_cmd): self.twist_cmd = twist_cmd 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): self.dbw_enabled = False 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, decel_limit, vehicle_mass, wheel_radius, brake_deadband) # TODO: Subscr8ibe to all the topics you need to rospy.Subscriber("/vehicle/dbw_enabled", Bool, self.toggle_dbw) rospy.Subscriber("/twist_cmd", TwistStamped, self.update_twist_cmd) rospy.Subscriber("/current_velocity", TwistStamped, self.update_current_velocity) self.current_twist = None self.current_velocity = None self.loop() def update_current_velocity(self, value): self.current_velocity = value.twist def toggle_dbw(self, value): self.controller.is_enabled = value.data def update_twist_cmd(self, value): self.current_twist = value.twist def loop(self): rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): if self.current_velocity and self.current_twist: throttle, brake, steering = self.controller.control( self.current_twist.linear.x, self.current_twist.angular.z, self.current_velocity.linear.x) if self.controller.is_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)
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node') self.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) self.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) self.brake_deadband = rospy.get_param('~brake_deadband', .1) self.decel_limit = rospy.get_param('~decel_limit', -5) self.accel_limit = rospy.get_param('~accel_limit', 1.) self.wheel_radius = rospy.get_param('~wheel_radius', 0.2413) self.wheel_base = rospy.get_param('~wheel_base', 2.8498) self.steer_ratio = rospy.get_param('~steer_ratio', 14.8) self.max_lat_accel = rospy.get_param('~max_lat_accel', 3.) self.max_steer_angle = rospy.get_param('~max_steer_angle', 8.) # steering self.steer_pub = rospy.Publisher('/vehicle/steering_cmd', SteeringCmd, queue_size=1) # throttle self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd', ThrottleCmd, queue_size=1) # brake self.brake_pub = rospy.Publisher('/vehicle/brake_cmd', BrakeCmd, queue_size=1) self.controller = Controller(self.vehicle_mass, self.fuel_capacity, self.brake_deadband, self.decel_limit, self.accel_limit, self.wheel_radius, self.wheel_base, self.steer_ratio, self.max_lat_accel, self.max_steer_angle) rospy.Subscriber('/final_waypoints', Lane, self.waypoints_cb) self.cte = 0. rospy.Subscriber('/cte', CTE, self.cte_cb) self.velocity = 0. rospy.Subscriber('/current_velocity', TwistStamped, self.vehicle_velocity_cb) # steering rospy.Subscriber('/actual/steering_cmd', SteeringCmd, self.actual_steer_cb) # throttle rospy.Subscriber('/actual/throttle_cmd', ThrottleCmd, self.actual_throttle_cb) # brake rospy.Subscriber('/actual/brake_cmd', BrakeCmd, self.actual_brake_cb) # Drive-by-Wire enabled notification self.dbw_enabled = False rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) self.target_speed = 0. self.loop() def waypoints_cb(self, msg): if len(msg.waypoints): self.target_speed = msg.waypoints[0].twist.twist.linear.x def vehicle_velocity_cb(self, msg): self.velocity = msg.twist.linear.x def cte_cb(self, msg): self.cte = msg.cte def loop(self): """ Main loop that periodically controls the vehicle """ rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): if self.dbw_enabled: throttle, brake, steering = self.controller.control(\ rospy.get_time(), self.target_speed, self.velocity, self.cte) rospy.logdebug("tgt v {:.2f} velocity {:.2f} cte {:.2f} thr {:.2f} brk {:.2f} str {:.2f}".format(\ self.target_speed, \ self.velocity, self.cte, throttle, brake, steering)) self.publish(throttle, brake, -steering) rate.sleep() def publish(self, throttle, brake, steer): """ Publishes throttle, brake and steering values to car :param throttle: throttle value :param brake: brake value :param steer: steering value """ # publish throttle value to /vehicle/throttle_cmd tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle self.throttle_pub.publish(tcmd) # publish steering value to /vehicle/steering_cmd scmd = SteeringCmd() scmd.enable = True scmd.steering_wheel_angle_cmd = steer self.steer_pub.publish(scmd) # publish brake value to /vehicle/brake_cmd 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): """ Callback for /vehicle/dbw_enabled topic subscriber :param msg: message from dbw_enabled topic """ self.dbw_enabled = msg.data if self.dbw_enabled: self.controller.reset(rospy.get_time(), self.cte) def actual_steer_cb(self, msg): """ Callback for /actual/steering topic subscriber :param msg: message from dbw_enabled topic """ if self.dbw_enabled and self.steer is not None: self.steer_data.append({ 'actual': msg.steering_wheel_angle_cmd, 'proposed': self.steer }) self.steer = None def actual_throttle_cb(self, msg): """ Callback for /actual/ topic subscriber :param msg: message from dbw_enabled topic """ if self.dbw_enabled and self.throttle is not None: self.throttle_data.append({ 'actual': msg.pedal_cmd, 'proposed': self.throttle }) self.throttle = None def actual_brake_cb(self, msg): """ Callback for /actual/ topic subscriber :param msg: message from dbw_enabled topic """ if self.dbw_enabled and self.brake is not None: self.brake_data.append({ 'actual': msg.pedal_cmd, 'proposed': self.brake }) self.brake = None
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): """ DBWNode is in charge of publishing all control values for the car. Using information about current state of the car ( velocity and position ) plus the desired trajectory ( velocity and path to follow ) it publishes the control values for the car: throttle, brake, steer. """ 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_pct = rospy.get_param('~max_throttle_percentage', 1.) rospy.logwarn("dbw_node: max throttle pct: %f", max_throttle_pct) max_braking_pct = rospy.get_param('~max_braking_percentage', -1.) rospy.logwarn("dbw_node: max braking pct: %f", max_braking_pct) self.lock = threading.Lock() self.dbw_enabled = False self.last_throttle = 2 * THROTTLE_EPSILON self.last_brake = 2 * BRAKE_EPSILON self.last_steer = 2 * STEERING_EPSILON self.activated = False self.current_velocity = None self.proposed_velocities = None self.current_pose = None self.waypoints = None 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, max_throttle_pct, max_braking_pct) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb, queue_size=SUBSCRIBER_QUEUE_SIZE) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb, queue_size=SUBSCRIBER_QUEUE_SIZE) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=SUBSCRIBER_QUEUE_SIZE) rospy.Subscriber('/current_pose', PoseStamped, self.current_pose_cb, queue_size=SUBSCRIBER_QUEUE_SIZE) rospy.Subscriber('/final_waypoints', Lane, self.waypoints_cb, queue_size=SUBSCRIBER_QUEUE_SIZE) self.loop() def loop(self): """Loop that computes throttle, brake and steer to publish.""" rate = rospy.Rate(50) while not rospy.is_shutdown(): if self._valid_state(): with self.lock: is_activated = self.activated cte = compute_cte(self.waypoints, self.current_pose) throttle, brake, steer = self.controller.control( is_activated, cte, self.proposed_velocities.twist.linear.x, self.proposed_velocities.twist.angular.z, self.current_velocity.twist.linear.x) if is_activated: rospy.logdebug("%f, %f, %f", throttle, brake, steer) self.publish(throttle, brake, steer) rate.sleep() def publish(self, throttle, brake, steer): """Publish throttle, brake and steer.""" if abs(throttle - self.last_throttle) < THROTTLE_EPSILON: tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle self.throttle_pub.publish(tcmd) else: rospy.logdebug("not publish throttle: %f", abs(throttle - self.last_throttle)) if abs(steer - self.last_steer) < STEERING_EPSILON: scmd = SteeringCmd() scmd.enable = True scmd.steering_wheel_angle_cmd = steer self.steer_pub.publish(scmd) else: rospy.logdebug("not publish steer: %f", abs(steer - self.last_steer)) if abs(brake - self.last_brake) < BRAKE_EPSILON: bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) else: rospy.logdebug("not publish brake: %f", abs(brake - self.last_brake)) self.last_throttle = throttle self.last_brake = brake self.last_steer = steer def current_velocity_cb(self, msg): self.current_velocity = msg def twist_cmd_cb(self, msg): self.proposed_velocities = msg def dbw_enabled_cb(self, msg): if (self.activated != msg.data): rospy.logwarn("%s has been %s", rospy.get_name(), "activated" if msg.data else "deactivated") self.activated = msg.data def current_pose_cb(self, msg): with self.lock: self.current_pose = msg.pose def waypoints_cb(self, msg): with self.lock: self.waypoints = msg.waypoints def _valid_state(self): """ Checks whether node has all information needed to operate correctly. This node needs: Proposed and current velocity, current position and waypoints to follow. """ return self.proposed_velocities is not None and self.current_velocity is not None and \ self.current_pose is not None and self.waypoints is not None
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') # Control scheme # Angle control is done by pure pursuit through Autoware # Linear control will be done through linear PID in twister_controller self.controller = Controller( vehicle_mass=rospy.get_param('~vehicle_mass', 1736.35), fuel_capacity=rospy.get_param('~fuel_capacity', 13.5), decel_limit=rospy.get_param('~decel_limit', -5.0), 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.), max_steer_angle=rospy.get_param('~max_steer_angle', 8.), min_speed=rospy.get_param('~min_speed', 0.0), linear_p_term=rospy.get_param('~linear_p_term', 0.3), linear_i_term=rospy.get_param('~linear_i_term', 0), linear_d_term=rospy.get_param('~linear_d_term', 0)) # Current command values self.steering = 0.0 self.throttle = 0.0 # Target velocities self.angular_velocity = 0.0 self.linear_velocity = 0.0 # DBW activation state self.dbw_enabled = 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) self.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.dbw_twist_cb) ] def dbw_enabled_cb(self, msg): self.dbw_enabled = msg.data def current_velocity_cb(self, msg): if not self.dbw_enabled: self.controller.reset() return command = self.controller.control( angular_velocity_setpoint=self.angular_velocity, linear_velocity_setpoint=self.linear_velocity, current_linear_velocity=msg.twist.linear.x) self.publish(*command) def dbw_twist_cb(self, msg): self.angular_velocity = msg.twist.angular.z self.linear_velocity = msg.twist.linear.x def spin(self): if rospy.get_param('~use_dbw', True): rospy.spin() def publish(self, throttle, brake, steering): if throttle > 0.0: self.publish_throttle(throttle) else: self.publish_brake(brake) self.publish_steering(steering) def publish_brake(self, brake): if abs(self.throttle + brake) < 1.0: return self.throttle = -brake bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) def publish_steering(self, steering): if abs(self.steering - steering) < 0.01: return self.steering = steering scmd = SteeringCmd() scmd.enable = True scmd.steering_wheel_angle_cmd = steering self.steer_pub.publish(scmd) def publish_throttle(self, throttle): if abs(self.throttle - throttle) < 0.01: return self.throttle = throttle tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle self.throttle_pub.publish(tcmd)
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node') # start with manual drive self._is_dbw_enabled = None self._current_vel_lin = None self._current_vel_ang = None self._des_lin_vel = None self._des_ang_vel = None 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.contoller = Controller(vehicle_mass, fuel_capacity, wheel_radius, decel_limit, accel_limit, max_steer_angle, wheel_base, steer_ratio, max_lat_accel) # Subscribe to all the topics you need to rospy.Subscriber('/twist_cmd', TwistStamped, self._get_twist_cmd) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self._set_bdw) rospy.Subscriber('/current_velocity', TwistStamped, self._get_current_vel) self.loop() def _set_bdw(self, is_dbw_enabled): self._is_dbw_enabled = is_dbw_enabled def _get_current_vel(self, curr_vel_twisted_stamped): self._current_vel_lin = curr_vel_twisted_stamped.twist.linear.x self._current_vel_ang = curr_vel_twisted_stamped.twist.angular.z def _get_twist_cmd(self, desired_speeds): self._des_lin_vel = desired_speeds.twist.linear.x self._des_ang_vel = desired_speeds.twist.angular.z def loop(self): rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): # Get predicted throttle, brake, and steering using `twist_controller` # You should only publish the control commands if dbw is enabled # todo remove rospy.loginfo("c_vel: %s, is_dbw: %s, des_lv: %s, des_av: %s", self._current_vel_lin, self._is_dbw_enabled, self._des_lin_vel, self._des_ang_vel) if self._current_vel_lin is not None and \ self._des_lin_vel is not None and \ self._des_ang_vel is not None and \ (self._is_dbw_enabled is not None): throttle, brake, steering = self.contoller.control( self._des_lin_vel, # proposedlinearvelocity self._des_ang_vel, # proposed angular velocity self._current_vel_lin # current linear velocity ) 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)
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.) 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.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(**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() # 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 if (self.current_velocity is not None) and (self.proposed_velocity is not None) and (self.final_waypoints is not None): 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 current_linear_velocity = self.current_velocity.twist.linear.x target_linear_velocity = self.proposed_velocity.twist.linear.x target_angular_velocity = self.proposed_velocity.twist.angular.z throttle, brake, steering = self.controller.control(target_linear_velocity, target_angular_velocity, current_linear_velocity, duration_in_seconds) 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: # rospy.logwarn('reset controller (DBW {}, current veolocity {}, proposed velocity {})'.format( # self.is_dbw_enabled, self.current_velocity.twist.linear.x, self.proposed_velocity.twist.linear.x)) 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, message): """ Message format: std_msgs/Header header uint32 seq time stamp string frame_id geometry_msgs/Twist twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z """ self.proposed_velocity = message def current_velocity_callback(self, message): """ Message format: std_msgs/Header header uint32 seq time stamp string frame_id geometry_msgs/Twist twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z """ self.current_velocity = message def dbw_enabled_callback(self, message): """ message: bool """ rospy.logwarn("DBW_ENABLED %s" % message) self.is_dbw_enabled = message.data def final_waypoints_cb(self, message): self.final_waypoints = message.waypoints def current_pose_cb(self, message): self.current_pose = message
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node') self.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) self.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) self.brake_deadband = rospy.get_param('~brake_deadband', .1) self.decel_limit = rospy.get_param('~decel_limit', -5) self.accel_limit = rospy.get_param('~accel_limit', 1.) self.wheel_radius = rospy.get_param('~wheel_radius', 0.2413) self.wheel_base = rospy.get_param('~wheel_base', 2.8498) self.steer_ratio = rospy.get_param('~steer_ratio', 14.8) self.max_lat_accel = rospy.get_param('~max_lat_accel', 3.) self.max_steer_angle = rospy.get_param('~max_steer_angle', 8.) self.min_speed = rospy.get_param('~min_speed', 4.*0.44704) self.max_throttle_percentage = rospy.get_param('~max_throttle_percentage', 0.1) self.max_braking_percentage = rospy.get_param('~max_braking_percentage', -0.1) 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.shutdown = False self.dbw_enabled = False self.current_velocity = 0. self.current_linear_velocity = 0. self.current_angular_velocity = 0. self.linear_velocity = 0. self.angular_velocity = 0. # TODO: Create `TwistController` object self.controller = Controller( self.wheel_base, self.steer_ratio, self.min_speed, self.accel_limit, self.max_steer_angle, self.vehicle_mass, self.wheel_radius, self.brake_deadband, self.max_throttle_percentage, self.max_braking_percentage) # TODO: Subscribe to all the topics you need to rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb) # go into event loop 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(self.linear_velocity, self.angular_velocity, self.current_linear_velocity, self.dbw_enabled) if self.dbw_enabled: self.publish(throttle, brake, steering) rate.sleep() def dbw_enabled_cb(self, msg): self.dbw_enabled = msg def velocity_cb(self, msg): self.current_linear_velocity = msg.twist.linear.x self.current_angular_velocity = msg.twist.angular.z print "velocity: ", msg print "" def twist_cb(self, msg): self.twist_cmd = msg self.linear_velocity = self.twist_cmd.twist.linear.x self.angular_velocity = self.twist_cmd.twist.angular.z print "twist_cmd:", msg print "" 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_PERCENT bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd)
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node') self.dbw_enabled = False self.linear_velocity = 0 self.angular_velocity = 0 self.current_velocity = 0 self.drive_by_wire_first_input_time = 0 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.max_braking_percent = rospy.get_param('~max_braking_percentage', -0.1) 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.dbw_enable_sub = rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_callback) self.current_velocity_sub = rospy.Subscriber( '/current_velocity', TwistStamped, self.current_velocity_callback) self.twist_cmd_sub = rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_callback) # TODO: Create `TwistController` object self.controller = Controller(wheel_base, steer_ratio, 0, max_lat_accel, max_steer_angle, accel_limit, decel_limit, vehicle_mass) # TODO: Subscribe to all the topics you need to 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>: if self.drive_by_wire_first_input_time == 0: self.drive_by_wire_first_input_time = time.time() * 1000 if self.dbw_enabled: throttle, brake, steering = self.controller.control( self.linear_velocity, self.angular_velocity, self.current_velocity) #giving sometime for tf and warm up first if time.time( ) * 1000 - self.drive_by_wire_first_input_time < 3000: throttle = 0.04 self.publish(throttle, brake, steering) 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 dbw_enabled_callback(self, data): self.dbw_enabled = data.data def current_velocity_callback(self, data): self.current_velocity = data.twist.linear.x def twist_cmd_callback(self, data): self.linear_velocity = data.twist.linear.x self.angular_velocity = data.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', 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') 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) '''config_string = rospy.get_param("/traffic_light_config") self.config = yaml.load(config_string) if self.config['type'] =='sim': throttle_limit = 0.7 tor_limit = 700 else: throttle_limit = 0.025 tor_limit = 0.25''' throttle_limit = 0.7 tor_limit = 700 self.controller = Controller(vehicle_mass, fuel_capacity, wheel_base, wheel_radius , steer_ratio,0, max_lat_accel, max_steer_angle, decel_limit, accel_limit, throttle_limit , tor_limit) # TODO: Subscribe to all the topics you need to rospy.Subscriber('/twist_cmd' , TwistStamped , self.twist_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool , self.dbw_enabled_cb) rospy.Subscriber('/current_velocity' , TwistStamped, self.vel_cb) self.dbw_enabled = None self.proposed_vel =0.0 self.proposed_angular_vel = 0.0 self.current_vel = 0.0 self.throttle = self.steering = self.brake =0 self.loop() def loop(self): rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): # You should only publish the control commands if dbw is enabled # TODO: Get predicted throttle, brake, and steering using `twist_controller` self.throttle, self.brake, self.steering = self.controller.control(self.current_vel, self.dbw_enabled, self.proposed_vel, self.proposed_angular_vel) if self.dbw_enabled: self.publish(self.throttle, self.brake, self.steering) rate.sleep() def publish(self, throttle, brake, steer): rospy.loginfo("Publishing Throttle : {0} , Brake : {1} , Steering : {2}".format(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.proposed_vel = msg.twist.linear.x self.proposed_angular_vel = msg.twist.angular.z def dbw_enabled_cb(self, msg): self.dbw_enabled = msg def vel_cb(self , msg): self.current_vel = msg.twist.linear.x
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, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle) # Subscribe to all the needed topics rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_callback) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_callback) rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_callback) self.dbw_enabled = None self.cmd_linear_velocity = None self.cmd_angular_velocity = None self.current_velocity = None self.current_angular_velocity = None # For handling cmd being too old self.cmd_time = None self.wheel_radius = wheel_radius self.vehicle_mass = vehicle_mass self.decel_limit = decel_limit self.loop() def loop(self): rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): # Get predicted throttle, brake, and steering using `twist_controller` # Only publish the control commands if dbw is enabled # Starting value throttle = steering = 0.0 brake = 700 # To prevent Carla from moving requires about 700 Nm of torque. # After getting at least one cmd if not None in (self.dbw_enabled, self.cmd_linear_velocity, self.cmd_angular_velocity): throttle, brake, steering = self.controller.control(self.dbw_enabled, self.cmd_linear_velocity, self.cmd_angular_velocity, self.current_velocity) # # cmd is too old, something may be wrong, stop the car # if (not self.cmd_time == None) and rospy.get_time() - self.cmd_time > 1: # throttle = steering = 0.0 # # torque = radius * force = radius * mass * acceleration # brake = self.wheel_radius * self.vehicle_mass * self.decel_limit # # # How to handle dbw_enabled being too old? if self.dbw_enabled: self.publish(throttle, brake, steering) rate.sleep() def dbw_enabled_callback(self, msg): self.dbw_enabled = msg def twist_cmd_callback(self, msg): self.cmd_linear_velocity = msg.twist.linear.x self.cmd_angular_velocity = msg.twist.angular.z self.cmd_time = msg.header.stamp.secs def current_velocity_callback(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.) max_acceleration = rospy.get_param('~max_acceleration', 1.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) # Create `TwistController` object # self.controller = TwistController(<Arguments you wish to provide>) kwargs = { "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_acceleration": max_acceleration } #rospy.logwarn(kwargs) self.controller = Controller(**kwargs) # Subscribe to all the topics you need to self.dbw_enabled = True self.waypoints = None self.twist = None self.velocity = None self.w = None self.current_pose = None rospy.Subscriber('/final_waypoints', Lane, self.waypoints_cb) 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) rospy.Subscriber('/current_pose', PoseStamped, self.current_pose_cb, queue_size=1) self.loop() 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) #rospy.logwarn(self.dbw_enabled) # If node isnt able to receive waypoints if self.waypoints is None: #rate.sleep() continue # If node isnt able to receive twist if self.twist is None: #rate.sleep() continue if self.current_pose is None: continue if self.velocity is None: continue #num_of_waypoints = len(self.waypoints) #rospy.logwarn(self.current_pose) # Calculate cte (to find steer) cte_args = { "waypoints": self.waypoints, "curpose": self.current_pose, "maxpoints": 10 } #cte = calculate_cte(**cte_args) cte = self.twist.angular.z #rospy.logwarn(cte) # Find: velocity, target velocity, ... v_target = self.twist.linear.x v = self.velocity w_target = self.twist.angular.z w = self.w # Control controller_args = { "cte": cte, "dbw_enabled": self.dbw_enabled, "v": v, "v_target": v_target, "w_target": w_target, "w": w } throttle, brake, steer = self.controller.control(**controller_args) # Publish control data to ROS topics # Is Manual Driving #rospy.logwarn(v) #rospy.logwarn(v_target) #rospy.logwarn(throttle) #rospy.logwarn(brake) if self.dbw_enabled: #rospy.logwarn("OK") self.publish(throttle, brake, steer) #else: #rospy.logwarn("Manual") 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 waypoints_cb(self, msg): # TODO: Change it later, Simple implementation only to test the partial waypoint node. #first_w = msg.waypoints[0] #rospy.loginfo('Received waypoints of size {}'.format(len(msg.waypoints))) #self.publish(1, 0, 0) self.waypoints = msg.waypoints def dbw_enabled_cb(self, msg): self.dbw_enabled = bool(msg.data) def twist_cb(self, msg): self.twist = msg.twist def velocity_cb(self, msg): self.velocity = msg.twist.linear.x self.w = msg.twist.angular.z def current_pose_cb(self, msg): self.current_pose = msg.pose
class DBWNode(object): def __init__(self): rospy.init_node('dbw_node') # Extract all parameters from ros server 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.) # Make a dictionary to pass the parameters on to the controller Parameters = { '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 } # 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) # Create controller object self.controller = Controller(**Parameters) # Class variables self.dbw_enabled = False self.current_velocity = None self.twist_cmd = None self.previous_time = rospy.get_time() # Subscribers rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity_cb, queue_size=1) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1) rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb, queue_size=5) self.loop() def loop(self): """loop""" rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): current_time = rospy.get_time() time_interval = current_time - self.previous_time if self.twist_cmd is not None and self.current_velocity is not None: throttle, brake, steering = self.controller.control( self.twist_cmd, self.current_velocity, time_interval) if self.dbw_enabled: self.publish(throttle, brake, steering) else: self.controller.reset() self.previous_time = current_time rate.sleep() def current_velocity_cb(self, msg): """current_velocity_cb Callback function for current_velocity :param msg: [geometry_msgs/TwistStamped]: std_msgs/Header header uint32 seq time stamp string frame_id geometry_msgs/Twist twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z """ self.current_velocity = msg def dbw_enabled_cb(self, msg): """dbw_enabled_cb Callback function for dbw_enabled :param msg: [std_msgs/Bool]: bool data """ self.dbw_enabled = msg def twist_cmd_cb(self, msg): """twist_cmd_cb Callback function for twist_cmd :param msg: [geometry_msgs/TwistStamped]: std_msgs/Header header uint32 seq time stamp string frame_id geometry_msgs/Twist twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z """ self.twist_cmd = msg def publish(self, throttle, brake, steer): """publish Publish the control values to the cmd nodes :param throttle: :param brake: :param 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.current_vel = None self.current_ang_vel = None self.dbw_enabled = None self.linear_vel = None self.angular_vel = None self.throttle = self.brake = self.steering = 0 # Creating the `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) 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) # Subscribing to the needed topics 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.loop() def loop(self): rate = rospy.Rate(FREQUENCY) while not rospy.is_shutdown(): # Getting the predicted throttle, brake, and steering using `twist_controller` if not None in (self.current_vel, self.linear_vel, self.angular_vel): self.throttle, self.brake, self.steering = self.controller.control( self.dbw_enabled, self.current_vel, self.linear_vel, self.angular_vel) # Only publishing the control commands if dbw is enabled 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.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('/current_velocity', TwistStamped, self.get_velocity_cb) rospy.Subscriber('/twist_cmd', TwistStamped, self.get_twist_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.get_dbw_enable_cb) self.velocity, self.twist, self.dbw_enable = None, None, None self.loop() def get_velocity_cb(self, v): self.velocity = v def get_twist_cb(self, twist): self.twist = twist def get_dbw_enable_cb(self, dbw_enable): self.dbw_enable = dbw_enable 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) <any other argument you need>) #if not initilized, have to wait if self.twist == None or self.velocity == None or self.dbw_enable == None: rate.sleep() continue # Need restart PID controller when switch to manual if self.dbw_enable.data == False: self.controller.resetpid() else: # Use rate = 50 to calculate accleration. throttle, brake, steer = self.controller.control( target_linear=self.twist.twist.linear, target_angular=self.twist.twist.angular, current_velocity=self.velocity, dbw_enable=self.dbw_enable.data, rate=50) if self.dbw_enable.data == True: 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)
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.current_vel = 0.0 self.target_angular_vel = 0.0 self.target_linear_vel = 0.0 # TODO: Create `TwistController` object min_speed = 0.0 self.controller = Controller(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) # TODO: Subscribe to all the topics you need to rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb) rospy.Subscriber('/twist_cmd', TwistStamped, self.angular_vel_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) rospy.Subscriber('/final_waypoints', Lane, self.final_waypoints_cb) self.dbw_enabled = False self.loop() rospy.spin() def velocity_cb(self, msg): self.current_vel = msg.twist.linear.x # m/s # print " [-] current velocity in m/s = %.3f" %(self.current_vel) def angular_vel_cb(self, msg): self.target_angular_vel = msg.twist.angular.z # rad/s def dbw_enabled_cb(self, msg): self.dbw_enabled = msg def final_waypoints_cb(self, msg): self.target_linear_vel = msg.waypoints[0].twist.twist.linear.x 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) # get current linear speed if self.target_angular_vel != None and self.current_vel != None: is_data = True else: is_data = False if is_data: throttle, brake, steering = self.controller.control( self.current_vel, self.target_linear_vel, self.target_angular_vel, self.dbw_enabled) if self.dbw_enabled: # print('Throttle: ' + str(throttle)) # print('Brake: ' + str(brake)) # print " [-] target steering wheel angle = %.2f (deg)" % (steering * 3.141592 / 180) 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)
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) self.dbw_enabled = False self.velocity = None self.cmd = None rospy.Subscriber('/twist_cmd', TwistStamped, self.cmd_cb) rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_cb) self.loop() def loop(self): rate = rospy.Rate(50) # 50Hz while not rospy.is_shutdown(): if self.velocity and self.cmd: throttle, brake, steering = self.controller.control( self.dbw_enabled, self.velocity.linear.x, self.cmd.linear.x, self.cmd.angular.z) 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 cmd_cb(self, msg): self.cmd = msg.twist def velocity_cb(self, msg): self.velocity = msg.twist def dbw_cb(self, msg): self.dbw_enabled = msg.data
class DBWNode(object): """ generate drive-by-wire(DBW) command for autonomous driving @subscribed /vehicle/dbw_enabled: the indicator for whether the car is under dbw or driver control @subscribed /current_velocity: the vehicle's target linear velocities @subscribed /twist_cmd: the vehicle's target angular velocities @published /vehicle/brake_cmd: the final brake for electronic control @published /vehicle/throttle_cmd: the final throttle for electronic control @published /vehicle/steering_cmd: the final steering for electronic control """ DBW_UPDATE_FREQ = 50 # Waypoint update frequency def __init__(self): rospy.init_node('dbw_node') # load ego 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.) # state variables: self.is_dbw_enabled = None self.actual_longitudinal_velocity = None self.target_longitudinal_velocity = None self.target_angular_velocity = None # 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: 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) # publish: 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.loop() def dbw_enabled_cb(self, is_dbw_enabled): """ update DBW activation status """ self.is_dbw_enabled = is_dbw_enabled def current_velocity_cb(self, current_velocity): """ update waypoint velocities """ self.actual_longitudinal_velocity = current_velocity.twist.linear.x def twist_cmd_cb(self, target_velocity): """ update twist command """ self.target_longitudinal_velocity = target_velocity.twist.linear.x self.target_angular_velocity = target_velocity.twist.angular.z def loop(self): """ The DBW system on Carla expects messages at 50Hz It will disengage (reverting control back to the driver) if control messages are published at less than 10hz """ rate = rospy.Rate(DBWNode.DBW_UPDATE_FREQ) # at least 50Hz while not rospy.is_shutdown(): # Get predicted throttle, brake, and steering using `twist_controller` throttle, brake, steer = self.controller.control( is_dbw_enabled=self.is_dbw_enabled, actual_longitudinal_velocity=self.actual_longitudinal_velocity, target_longitudinal_velocity=self.target_longitudinal_velocity, target_angular_velocity=self.target_angular_velocity) # Only publish the control commands if dbw is enabled if self.is_dbw_enabled: self.publish(throttle, brake, steer) rate.sleep() def publish(self, throttle, brake, steer): """ publish drive-by-wire(DBW) control command for autonomous driving """ # throttle: tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle self.throttle_pub.publish(tcmd) # steering: scmd = SteeringCmd() scmd.enable = True scmd.steering_wheel_angle_cmd = steer self.steer_pub.publish(scmd) # brake: 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) 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) 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.1 self.process_rate = 5 self.send_rate = 50 # must be a multiple of the processing rate, change to 10 for the simulator self.process_itr = 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(<Arguments you wish to provide>) self.dbw_controller = Controller(vehicle_mass, wheel_base, wheel_radius, steer_ratio, min_speed, max_lat_accel, max_steer_angle, accel_limit, decel_limit, self.process_rate) # TODO: Subscribe to all the topics you need to 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.linear_velocity = None self.angular_velocity = None self.dbw_enabled = None self.throttle = self.steering = 0 self.brake = 700 self.loop() def loop(self): rate = rospy.Rate(self.send_rate) # 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.process_itr % (self.send_rate / self.process_rate) == 0 ) and not None in (self.current_velocity, self.linear_velocity, self.angular_velocity): self.throttle, self.brake, self.steering = self.dbw_controller.control( self.current_velocity, self.dbw_enabled, self.linear_velocity, self.angular_velocity) self.process_itr = 0 if self.dbw_enabled: self.publish(self.throttle, self.brake, self.steering) self.process_itr = self.process_itr + 1 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_cb(self, msg): self.linear_velocity = msg.twist.linear.x self.angular_velocity = msg.twist.angular.z def current_vel_cb(self, msg): self.current_velocity = msg.twist.linear.x def dbw_enabled_cb(self, msg): self.dbw_enabled = msg
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.) 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, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, 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.current_angular_vel = None self.linear_vel = None self.angular_vel = None self.dbw_enabled = None self.throttle = 0 self.steering = 0 self.brake = 0 self.loop() def loop(self): rate = rospy.Rate(50) 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.angular_vel, self.linear_vel, self.dbw_enabled) # Publish only if dbw is enabled or else it is manual control if self.dbw_enabled: self.publish(self.throttle, self.brake, self.steering) rate.sleep() def dbw_enabled_cb(self, msg): self.dbw_enabled = msg.data if not self.dbw_enabled: rospy.logwarn('Manual Mode Enabled') else: rospy.logwarn('Autonomous Mode Enabled') 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) 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