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)
kp = rospy.get_param('~k_p', 0.05) # 1.
ki = rospy.get_param('~k_i', 0.25) # 5.
kd = rospy.get_param('~k_d', 0.00075) # 0.02
pid_min = -1
pid_max = 1
max_brake_cmd = min(-decel_limit,
2.) * vehicle_mass * wheel_radius ## N.m
# TODO: Create `TwistController` object
self.controller = Controller(wheel_base, steer_ratio, 0, max_lat_accel,
max_steer_angle, kp, ki, kd, pid_min,
pid_max, max_brake_cmd)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/current_velocity', TwistStamped, self.velo_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_cb)
self.current_vel = 0
self.current_vel_time = 0
self.lin_tgt_vel = 0
self.ang_tgt_vel = 0
self.dbw_enabled = False
self.loop()
def __init__(self):
rospy.init_node('dbw_node')
# Obtaining the necessary parameters to be initilised
accel_limit = rospy.get_param('~accel_limit', 1.)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
# TODO: Create `Controller` object
# Defining the parameters of a "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)
# Initilising the parameters of object self
self.angular_vel = None
self.current_vel = None
self.curr_ang_vel = None
self.dbw_enabled = None
self.linear_vel = None
self.throttle = self.steering = self.brake = 0
# Code for publishing the messages SteeringCmd, BrakeCmd, ThrottleCmd
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd, queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd, queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd, queue_size=1)
# 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.loop()
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', 0.1)
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.0)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.0)
self.twist = None
self.current_vel = None
self.dbw_enabled = False
self.target_linear_velocity = None
self.current_linear_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)
# 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,
min_speed=0.1)
# Subscribe to the relevant topics.
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_velocity_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
self.loop()
def __init__(self):
# Initialize node
rospy.init_node('dbw_node')
# Get vehicle parameters
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
# Create publishers
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# Create `Controller` object
self.controller = Controller(vehicle_mass, brake_deadband, decel_limit,
accel_limit, wheel_radius, wheel_base,
steer_ratio, max_lat_accel,
max_steer_angle)
self.cur_longitudinal_velocity = None
self.cur_angular_velocity = None
self.dbw_enabled = None
self.longitudinal_velocity = None
self.angular_velocity = None
self.throttle = 0.0
self.steering = 0.0
self.braking = 0.0
# Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_cb)
rospy.Subscriber('/current_velocity', TwistStamped,
self.cur_velocity_cb)
self.loop()