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.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 = TwistController(vehicle_mass, fuel_capacity,
brake_deadband, decel_limit,
accel_limit, wheel_radius,
wheel_base, steer_ratio,
max_lat_accel, max_steer_angle,
min_speed)
self.dbw_enabled = True
self.reset_controller = True
self.current_velocity_msg = None
self.twist_cmd = None
self.current_velocity = 0.0
self.current_angualr_velocity = 0.0
self.twist_velocity = 0.0
self.twist_angualr_velocity = 0.0
self.prev_time = rospy.get_time()
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=5)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=5)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=1)
self.loop()
def __init__(self):
rospy.init_node('dbw_node')
rospy.loginfo('DBW Node Initialized')
vehicle_params = {
'vehicle_mass': rospy.get_param('~vehicle_mass', 1736.35), # kg
'fuel_capacity': rospy.get_param('~fuel_capacity', 13.5), # kg/gal
'brake_deadband': rospy.get_param('~brake_deadband', .1),
'decel_limit': rospy.get_param('~decel_limit', -5),
'accel_limit': rospy.get_param('~accel_limit', 1.), # 1 g?
'wheel_radius': rospy.get_param('~wheel_radius', 0.2413),
'wheel_base': rospy.get_param('~wheel_base', 2.8498),
'steer_ratio': rospy.get_param('~steer_ratio', 14.8),
'max_lat_accel': rospy.get_param('~max_lat_accel', 3.),
'max_steer_angle': rospy.get_param('~max_steer_angle', 8.),
'min_speed': 0.0,
}
self.dbw_enabled = False
self.actual_velocity = None
self.proposed_command = None
self.previous_timestamp = rospy.get_time()
self.controller = TwistController(vehicle_params)
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)
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)
self.loop()
def __init__(self):
# params
self.started_components = False
self.event = None
self.pose_monitor_feedback = None
self.pose_2 = None
self.has_pose_error_data = False
self.transform_to_pose_converter = TransformToPoseConverter()
self.component_wise_pose_error_calculator = ComponentWisePoseErrorCalculator(
)
self.component_wise_pose_error_monitor = ComponentWisePoseErrorMonitor(
)
self.feedback = mcr_monitoring_msgs.msg.ComponentWisePoseErrorMonitorFeedback(
)
self.twist_controller = TwistController()
self.twist_limiter = TwistLimiter()
self.twist_synchronizer = TwistSynchronizer()
# node cycle rate (in hz)
self.loop_rate = rospy.Rate(rospy.get_param('~loop_rate', 100.0))
self.near_zero = rospy.get_param('~near_zero', 0.001)
# publishers
self.event_out = rospy.Publisher("~event_out",
std_msgs.msg.String,
queue_size=1)
self.dynamic_reconfigure_server = dynamic_reconfigure.server.Server(
ComponentWisePoseErrorMonitorConfig, self.dynamic_reconfigure_cb)
self.pose_error_monitor_event_in = rospy.Publisher(
'~pose_error_monitor_event_in',
std_msgs.msg.String,
latch=True,
queue_size=1)
self.base_twist = rospy.Publisher('~twist',
geometry_msgs.msg.Twist,
queue_size=1)
# Setup for component_wise_pose_error_calculator
# subscribers
rospy.Subscriber("~event_in", std_msgs.msg.String, self.event_in_cb)
rospy.Subscriber(
"~pose_monitor_feedback",
mcr_monitoring_msgs.msg.ComponentWisePoseErrorMonitorFeedback,
self.pose_monitor_feedback_cb)
rospy.Subscriber('~pose_2', geometry_msgs.msg.PoseStamped,
self.pose_2_cb)
rospy.Subscriber(
'~pose_error',
mcr_manipulation_msgs.msg.ComponentWiseCartesianDifference,
self.pose_error_cb)
def __init__(self):
rospy.init_node('dbw_node', log_level=LOG_LEVEL)
# Fetch value from parameter 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.)
max_throttle_percentage = rospy.get_param('~max_throttle_percentage',
0.8)
max_brake_percentage = rospy.get_param('~max_brake_percentage', -0.8)
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,
'max_throttle_percentage': max_throttle_percentage,
'max_brake_percentage': max_brake_percentage
}
self.current_velocity = None
self.twist_cmd = None
self.dbw_enabled = False
self.twist_controller = TwistController(**params)
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.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 __init__(self):
rospy.init_node('dbw_node')
cp = CarParams()
cp.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
cp.fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
cp.brake_deadband = rospy.get_param('~brake_deadband', .1)
cp.decel_limit = rospy.get_param('~decel_limit', -5)
cp.accel_limit = rospy.get_param('~accel_limit', 1.)
cp.wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
cp.wheel_base = rospy.get_param('~wheel_base', 2.8498)
cp.steer_ratio = rospy.get_param('~steer_ratio', 14.8)
cp.max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
cp.max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
cp.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.latest_twist_cmd = None
# TIME
self.previous_timestamp = rospy.get_time()
# Create `TwistController` object
self.controller = TwistController(cp=cp)
# Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=5)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=5)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=1)
self.loop()
def __init__(self):
rospy.init_node('dbw_node')
self.current_linear_velocity = 0.0
self.target_linear_velocity = 0.0
self.current_angular_velocity = 0.0
self.target_angular_velocity = 0.0
self.dbw_enabled = False
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.) # meters/s^2
max_steer_angle = rospy.get_param('~max_steer_angle',
8.) #=458 degrees
# steer:wheel = 14.8:1,
# max_steer_angle = 8 rads = 458 degrees
# 458/14.8 = ~30 degrees (max steering range: -15 to 15)
# steering_sensitivity is in radians^(-1)
# this number is used to get the noramlized steering
self.steering_sensitivity = steer_ratio / max_steer_angle * 2.0
#rospy.logerr('st_sen:{} {} {}'.format( steer_ratio, max_steer_angle, self.steering_sensitivity))
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
self.controller = TwistController() #(<Arguments you wish to provide>)
#max_steer_angle is half as in yaw_controller, we have range of (-1* max_steer_angle to max_steer_angle)
self.controller.yaw_controller = YawController(wheel_base, steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_cb)
rospy.spin() # ===> use spin, publishing cmd right away in twist_cb
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.dbw_enabled = False
self.tl_detector_ready = False
self.target_velocity = 0.
self.target_yaw_dot = 0.
self.current_velocity = 0.
self.current_yaw_dot = 0.
self.last_update_time = None
# Initialize TwistController
self.twist_controller = TwistController(accel_limit, -1.,
max_steer_angle,
BrakeCmd.TORQUE_MAX,
wheel_base, steer_ratio,
max_lat_accel, max_steer_angle)
# Subscribe to topics
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_callback)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_velocity_callback)
rospy.Subscriber('/vehicle/dbw_enabled', Bool,
self.dbw_enabled_callback)
rospy.Subscriber('/tl_detector_ready', Bool,
self.tl_detector_ready_callback)
self.loop()
def __init__(self):
rospy.init_node('dbw_node')
parameters = dict()
parameters['vehicle_mass'] = rospy.get_param('~vehicle_mass', 1736.35)
parameters['fuel_capacity'] = rospy.get_param('~fuel_capacity', 13.5)
parameters['dbrake_deadband'] = rospy.get_param('~brake_deadband', .1)
parameters['decel_limit'] = rospy.get_param('~decel_limit', -5)
parameters['accel_limit'] = rospy.get_param('~accel_limit', 1.)
parameters['wheel_radius'] = rospy.get_param('~wheel_radius', 0.2413)
parameters['wheel_base'] = rospy.get_param('~wheel_base', 2.8498)
parameters['steer_ratio'] = rospy.get_param('~steer_ratio', 14.8)
parameters['max_lat_accel'] = rospy.get_param('~max_lat_accel', 3.)
parameters['max_steer_angle'] = rospy.get_param('~max_steer_angle', 8.)
parameters['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.dbw_enabled = True
self.current_twist = None
self.twist_cmd = None
self.t_0 = rospy.get_time()
# TODO: Create `TwistController` object
self.controller = TwistController(parameters=parameters)
# Subscribe to all the needed topics
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=5)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=5)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=1)
self.loop()
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.brake_deadband = brake_deadband
self.decel_limit = decel_limit
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.twist_controller = TwistController(max_steer_angle, accel_limit,
decel_limit)
self.gain_controller = GainController(max_throttle=1.0,
max_brake=1.0,
max_steer_angle=max_steer_angle,
delay_seconds=1.0,
steer_ratio=steer_ratio)
self.goal_acceleration = 0
self.goal_yaw_rate = 0.
self.current_linear = [0, 0]
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_callback)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_velocity_callback)
rospy.Subscriber('/vehicle/dbw_enabled', Bool,
self.dbw_enabled_callback)
srv = Server(PIDParamsConfig, self.config_callback)
self.loop()
def __init__(self):
rospy.init_node('dbw_node', log_level=rospy.INFO)
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
self.controller = TwistController(
YawController(wheel_base, steer_ratio, MIN_SPEED, max_lat_accel,
max_steer_angle), accel_limit, decel_limit,
vehicle_mass)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.enabled_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
self.dbw_enabled = False
self.velocity = None
self.proposed_linear = None
self.proposed_angular = None
self.loop()
def __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()
# TODO: Subscribe to all the topics you need to
self.dbw_enabled = True
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_cb)
self.loop()
def __init__(self):
rospy.init_node('dbw_node')
vehicle_capabilites = {
'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)
# Set the initial stage prior to recieving any messages.
self.dbw_enabled = False
self.latest_current_velocity_msg = None
self.latest_twist_msg = None
self.controller = TwistController(**vehicle_capabilites)
# Messages arrive async, each callback updates this
# class with private copy of last update seen.
#Topic to receive target linear and angular velocities
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_callback)
# Subscribe to current velocity topic.
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_vel_callback)
#Messge that indicates if dbw is enabled or not.
rospy.Subscriber('/vehicle/dbw_enabled', Bool,
self.dbw_enabled_callback)
self.loop()
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
min_speed = 0.0
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# Create `TwistController` object
self.controller = TwistController(vehicle_mass, fuel_capacity, brake_deadband, \
decel_limit, accel_limit, wheel_radius, \
wheel_base, steer_ratio, max_lat_accel, \
max_steer_angle)
self.current_velocity = None
self.cmd_linear_velocity = None
self.cmd_angular_velocity = None
self.dbw_enabled = False
self.previous_time = None
self.pub_last_time = rospy.get_time()
self.name = self.__class__.__name__
# You will need to write ROS subscribers for the /current_velocity, /twist_cmd, and /vehicle/dbw_enabled topics
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 __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(vehicle_mass, 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
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)
self.dbw_enabled_sub = rospy.Subscriber("/vehicle/dbw_enabled", Bool, self.dbw_enabled_callback)
self.pose_sub = rospy.Subscriber('/current_pose', PoseStamped, self.pose_callback)
self.waypoint_sub = rospy.Subscriber('final_waypoints', Lane, self.waypoint_callback)
#set up class variables to store data from subscribers
self.current_velocity = 0.0
self.velocity_cmd = 0.0
self.angular_velocity_cmd = 0.0
self.dbw_enabled = False
self.car_position = [0, 0, 0]
self.waypoint_position = [0, 0, 0]
#set up timestamp for measuring actual cycle time
self.time = rospy.get_time()
self.loop()
def __init__(self):
rospy.init_node('dbw_node')
#rospy.loginfo('dbw node init')
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=30)
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(wheel_base, wheel_radius,
vehicle_mass, steer_ratio,
max_lat_accel, max_steer_angle)
# TODO: Subscribe to all the topics you need to
sub1 = rospy.Subscriber("/twist_cmd", TwistStamped,
self.twistCmdCallback)
sub2 = rospy.Subscriber("/current_velocity", TwistStamped,
self.currentVelCallback)
sub3 = rospy.Subscriber("/vehicle/dbw_enabled", Bool,
self.enabledCallback)
self.target_linear_vel = 0
self.current_vel = 0
self.target_angular_vel = 0
self.dbw_enabled = False
self.loop()
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
# TODO: Create `TwistController` object
#self.controller = TwistController(<Arguments you wish to provide>)
min_speed = .1 # TODO made up value, what is needed?
self.controller = TwistController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle,
decel_limit, accel_limit)
# 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)
# Maybe subscribe to /final_waypoints for now, until following is complete
#rospy.Subscriber('/final_waypoints', Lane, self.final_waypoints_cb)
self.twist_cmd = None
self.current_velocity = None
self.dbw_enabled = None
self.loop()
def __init__(self):
rospy.init_node('dbw_node')
self.dbw_enabled = False
self.current_twist_command = None
self.current_velocity = None
self.initialized = False
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 = TwistController(wheel_base=wheel_base,
steer_ratio=steer_ratio,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle,
min_speed=0,
vehicle_mass=vehicle_mass,
decel_limit=decel_limit,
accel_limit=accel_limit,
wheel_radius=wheel_radius,
fuel_capacity=fuel_capacity)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.velocity_cb,
queue_size=1)
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_command_cb,
queue_size=1)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_status_cb,
queue_size=1)
self.previous_time = rospy.get_time()
self.loop()
def __init__(self):
rospy.init_node('dbw_node')
self.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.)
self.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 subscribers
self.sub_current_velocity = rospy.Subscriber(
"/current_velocity", TwistStamped, self.current_velocity_callback)
self.sub_twist_cmd = rospy.Subscriber("/twist_cmd", TwistStamped,
self.twist_cmd_callback)
self.sub_dbw_enabled = rospy.Subscriber("/vehicle/dbw_enabled", Bool,
self.dbw_enabled_callback)
self.twist_controller = TwistController(wheel_base, steer_ratio,
max_lat_accel, max_steer_angle,
decel_limit)
self.dbw_enabled = False
self.current_velocity = 0
self.desired_angular_velocity = 0
self.desired_long_velocity = 0
self.loop()
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
self.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.)
self.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 subscribers
self.sub_current_velocity = rospy.Subscriber(
"/current_velocity", TwistStamped, self.current_velocity_callback)
self.sub_twist_cmd = rospy.Subscriber("/twist_cmd", TwistStamped,
self.twist_cmd_callback)
self.sub_dbw_enabled = rospy.Subscriber("/vehicle/dbw_enabled", Bool,
self.dbw_enabled_callback)
self.twist_controller = TwistController(wheel_base, steer_ratio,
max_lat_accel, max_steer_angle,
decel_limit)
self.dbw_enabled = False
self.current_velocity = 0
self.desired_angular_velocity = 0
self.desired_long_velocity = 0
self.loop()
def deceleration_to_Nm(self, desired_deceleration):
brake_torque = self.vehicle_mass * self.wheel_radius * desired_deceleration
return brake_torque
def current_velocity_callback(self, twist_msg):
# current longitudinal velocity
self.current_velocity = twist_msg.twist.linear.x
def twist_cmd_callback(self, twist_msg):
# desired angular_velocity
self.desired_angular_velocity = twist_msg.twist.angular.z
# desired longitudinal velocity
self.desired_long_velocity = twist_msg.twist.linear.x
def dbw_enabled_callback(self, bool_msg):
self.dbw_enabled = bool_msg.data
rospy.logwarn('self.dbw_enabled = %d', self.dbw_enabled)
return
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
if self.dbw_enabled:
throttle, deceleration, steering = self.twist_controller.control(
self.current_velocity, self.desired_long_velocity,
self.desired_angular_velocity)
#rospy.logwarn('current_velocity: %f, desired_velocity: %f, throttle: %f, brake: %f',
# self.current_velocity, self.desired_long_velocity, throttle, deceleration)
# throttle message
throttle_msg = ThrottleCmd()
throttle_msg.enable = True
throttle_msg.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
throttle_msg.pedal_cmd = throttle # 0 ... 1
self.throttle_pub.publish(throttle_msg)
# brake message
brake_msg = BrakeCmd()
brake_msg.enable = True
brake_msg.pedal_cmd_type = BrakeCmd.CMD_TORQUE # Nm, range 0 to 3250
if deceleration > 0:
brake_msg.boo_cmd = True
brake_msg.pedal_cmd = self.deceleration_to_Nm(deceleration)
else:
brake_msg.boo_cmd = False
brake_msg.pedal_cmd = 0
self.brake_pub.publish(brake_msg)
# steering message
steering_msg = SteeringCmd()
steering_msg.enable = True
steering_msg.steering_wheel_angle_cmd = steering # in rad, range -8.2 to 8.2
self.steer_pub.publish(steering_msg)
rate.sleep()
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def __init__(self):
rospy.init_node('dbw_node')
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.max_vel_mps = rospy.get_param(
'waypoint_loader/velocity') * MPH_to_MPS
self.loop_freq = rospy.get_param('~loop_freq', 2)
# the frequency to process vehicle messages
self.desired_velocity, self.current_velocity = None, 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)
# DONE: Create `TwistController` object
self.controller = TwistController(vehicle_mass=self.vehicle_mass,
fuel_capacity=self.fuel_capacity,
brake_deadband=self.brake_deadband,
decel_limit=self.decel_limit,
accel_limit=self.accel_limit,
wheel_radius=self.wheel_radius,
wheel_base=self.wheel_base,
steer_ratio=self.steer_ratio,
max_lat_accel=self.max_lat_accel,
max_steer_angle=self.max_steer_angle)
# min_speed=self.min_speed,
# ,
# max_braking_percentage=self.max_braking_percentage,
# max_throttle_percentage=self.max_throttle_percentage,
# max_vel_mps=self.max_vel_mps
# DONE: Subscribe to all the topics you need to
self.dbw_enabled = False
self.current_velocity = None
self.desired_velocity = None
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()
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node', log_level=LOG_LEVEL)
# Fetch value from parameter 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.)
max_throttle_percentage = rospy.get_param('~max_throttle_percentage',
0.8)
max_brake_percentage = rospy.get_param('~max_brake_percentage', -0.8)
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,
'max_throttle_percentage': max_throttle_percentage,
'max_brake_percentage': max_brake_percentage
}
self.current_velocity = None
self.twist_cmd = None
self.dbw_enabled = False
self.twist_controller = TwistController(**params)
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.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 loop(self):
rate = rospy.Rate(DBW_CONTROLLER_RATE)
while not rospy.is_shutdown():
if self.current_velocity and self.twist_cmd:
# Run twist controller to get throttle brake and steer
throttle, brake, steer = self.twist_controller.control(
proposed_twist=self.twist_cmd.twist,
current_twist=self.current_velocity.twist,
dbw_enabled=self.dbw_enabled)
rospy.loginfo("throttle=%.3f, brake=%.3f, steer=%.3f",
throttle, brake, steer)
# Publish only if dbw_enabled
if self.dbw_enabled:
self.publish(throttle, brake, steer)
rate.sleep()
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def current_velocity_cb(self, msg):
self.current_velocity = msg
def twist_cmd_cb(self, msg):
self.twist_cmd = msg
# rospy.loginfo(msg)
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
min_speed = 0.0
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd, queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd, queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd, queue_size=1)
# Create `TwistController` object
self.controller = TwistController(vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, \
steer_ratio, max_lat_accel, max_steer_angle, min_speed)
self.current_velocity = None
self.twist_cmd = None
self.dbw_enabled = False
self.previous_time = None
# You will need to write ROS subscribers for the /current_velocity, /twist_cmd, and /vehicle/dbw_enabled topics
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
#rate = rospy.Rate(10)
while not rospy.is_shutdown():
if (self.current_velocity is not None) and (self.twist_cmd is not None):
current_time = rospy.get_time()
dt = (current_time - self.previous_time) if (self.previous_time is not None) else 0
self.previous_time = current_time
proposed_linear_velocity = self.twist_cmd.twist.linear
proposed_angular_velocity = self.twist_cmd.twist.angular
self.controller.enable(self.dbw_enabled)
# Get predicted throttle, brake, and steering using `twist_controller`
throttle, brake, steering = self.controller.control(proposed_linear_velocity,
proposed_angular_velocity,
self.current_velocity,
self.dbw_enabled, dt)
# You should only publish the control commands if dbw is 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.data
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.max_vel_mps = rospy.get_param(
'waypoint_loader/velocity') * MPH_to_MPS
self.loop_freq = rospy.get_param('~loop_freq', 2)
# the frequency to process vehicle messages
self.desired_velocity, self.current_velocity = None, 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)
# DONE: Create `TwistController` object
self.controller = TwistController(vehicle_mass=self.vehicle_mass,
fuel_capacity=self.fuel_capacity,
brake_deadband=self.brake_deadband,
decel_limit=self.decel_limit,
accel_limit=self.accel_limit,
wheel_radius=self.wheel_radius,
wheel_base=self.wheel_base,
steer_ratio=self.steer_ratio,
max_lat_accel=self.max_lat_accel,
max_steer_angle=self.max_steer_angle)
# min_speed=self.min_speed,
# ,
# max_braking_percentage=self.max_braking_percentage,
# max_throttle_percentage=self.max_throttle_percentage,
# max_vel_mps=self.max_vel_mps
# DONE: Subscribe to all the topics you need to
self.dbw_enabled = False
self.current_velocity = None
self.desired_velocity = None
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(self.loop_freq) # from 50Hz to 2Hz
dt = 1. / self.loop_freq
while not rospy.is_shutdown():
if self.desired_velocity and self.current_velocity:
# DONE: Get predicted throttle, brake, and steering using `twist_controller`
# You should only publish the control commands if dbw is enabled
if self.dbw_enabled:
throttle, brake, steering = self.controller.control(
self.desired_velocity, self.current_velocity, dt)
self.publish(throttle, brake, steering)
# end of self.dbw_enabled
self.desired_velocity, self.current_velocity = None, None
# end of if self.desired_velocity and self.current_velocity
rate.sleep()
def current_velocity_cb(self, msg):
if self.current_velocity is None:
self.current_velocity = msg # .twist
# end of if self.current_velocity is None:
# self.current_velocity = msg.twist
def twist_cmd_cb(self, msg):
self.desired_velocity = msg # .twist
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg.data
if (self.dbw_enabled):
self.controller.velocity_pid.reset()
# end of def dbw_enabled_cb(self, msg)
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.current_linear_velocity = 0.0
self.target_linear_velocity = 0.0
self.current_angular_velocity = 0.0
self.target_angular_velocity = 0.0
self.dbw_enabled = False
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.) # meters/s^2
max_steer_angle = rospy.get_param('~max_steer_angle',
8.) #=458 degrees
# steer:wheel = 14.8:1,
# max_steer_angle = 8 rads = 458 degrees
# 458/14.8 = ~30 degrees (max steering range: -15 to 15)
# steering_sensitivity is in radians^(-1)
# this number is used to get the noramlized steering
self.steering_sensitivity = steer_ratio / max_steer_angle * 2.0
#rospy.logerr('st_sen:{} {} {}'.format( steer_ratio, max_steer_angle, self.steering_sensitivity))
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
self.controller = TwistController() #(<Arguments you wish to provide>)
#max_steer_angle is half as in yaw_controller, we have range of (-1* max_steer_angle to max_steer_angle)
self.controller.yaw_controller = YawController(wheel_base, steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb)
rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_cb)
rospy.spin() # ===> use spin, publishing cmd right away in twist_cb
def dbw_cb(self, msg):
if (self.dbw_enabled != msg.data):
self.dbw_enabled = msg.data
pass
def velocity_cb(self, msg):
self.current_linear_velocity = msg.twist.linear.x
self.current_angular_velocity = msg.twist.angular.z
pass
def twist_cb(self, msg):
self.target_linear_velocity = msg.twist.linear.x
self.target_angular_velocity = msg.twist.angular.z
target_linear_velocity = self.target_linear_velocity * MPH_2_mps
if self.dbw_enabled: #<dbw is enabled>:
throttle, brake, steer = self.controller.control(
self.current_linear_velocity, target_linear_velocity,
self.steering_sensitivity, self.target_angular_velocity)
self.publish(throttle, brake, steer)
#if brake > 10:
# rospy.logerr('dbw_node:{: f}\t{: f}\t{: f}'.format(steer, throttle, brake))
pass
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)
''' brake command will set throttle value to 0, even if brake is zero'''
''' therefore the brake command is only sent if we are braking '''
if brake > 0:
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
min_speed = 0.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 `TwistController` object
# self.controller = TwistController(<Arguments you wish to provide>)
self.controller = TwistController(vehicle_mass, fuel_capacity,
brake_deadband, decel_limit,
accel_limit, wheel_radius,
wheel_base, steer_ratio,
max_lat_accel, max_steer_angle,
min_speed)
self.dbw_enabled = True
self.reset_controller = True
self.current_velocity_msg = None
self.twist_cmd = None
self.current_velocity = 0.0
self.current_angualr_velocity = 0.0
self.twist_velocity = 0.0
self.twist_angualr_velocity = 0.0
self.prev_time = rospy.get_time()
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_cmd_cb,
queue_size=5)
rospy.Subscriber('/current_velocity',
TwistStamped,
self.current_velocity_cb,
queue_size=5)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_enabled_cb,
queue_size=1)
self.loop()
def twist_cmd_cb(self, msg):
self.twist_cmd = msg
self.twist_velocity = msg.twist.linear.x
self.twist_angualr_velocity = msg.twist.angular.z
def current_velocity_cb(self, msg):
self.current_velocity_msg = msg
self.current_velocity = msg.twist.linear.x
self.current_angualr_velocity = msg.twist.angular.z
def dbw_enabled_cb(self, msg):
try:
self.dbw_enabled = bool(msg.data)
except Exception:
self.dbw_enabled = msg
def loop(self):
rate = rospy.Rate(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)
# TIME
now = rospy.get_time()
dt = now - self.prev_time
self.prev_time = now
if self.dbw_enabled and self.current_velocity_msg is not None and self.twist_cmd is not None:
if self.reset_controller:
self.controller.reset()
self.reset_controller = False
throttle, brake, steering = self.controller.control(
twist_velocity=self.twist_velocity,
twist_angualr_velocity=self.twist_angualr_velocity,
current_velocity=self.current_velocity,
delta_time=dt)
self.publish(throttle, brake, steering)
else:
self.reset_controller = True
rate.sleep()
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
#rospy.loginfo('dbw node init')
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=30)
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(wheel_base, wheel_radius,
vehicle_mass, steer_ratio,
max_lat_accel, max_steer_angle)
# TODO: Subscribe to all the topics you need to
sub1 = rospy.Subscriber("/twist_cmd", TwistStamped,
self.twistCmdCallback)
sub2 = rospy.Subscriber("/current_velocity", TwistStamped,
self.currentVelCallback)
sub3 = rospy.Subscriber("/vehicle/dbw_enabled", Bool,
self.enabledCallback)
self.target_linear_vel = 0
self.current_vel = 0
self.target_angular_vel = 0
self.dbw_enabled = False
self.loop()
def loop(self):
#rospy.loginfo('dbw node loop called')
#rate = rospy.Rate(10) # 10Hz
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
#rospy.logdebug('dbw node while loop start')
# 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>)
throttle, brake, steering = self.controller.control(
self.target_linear_vel, self.target_angular_vel,
self.current_vel)
rospy.loginfo('dbw node in loop before enable')
if self.dbw_enabled:
rospy.loginfo('dbw node enabled publish')
self.publish(throttle, brake, steering)
rate.sleep()
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def twistCmdCallback(self, msg):
self.target_linear_vel = msg.twist.linear.x
self.target_angular_vel = msg.twist.angular.z
def currentVelCallback(self, msg):
self.current_vel = msg.twist.linear.x
def enabledCallback(self, msg):
rospy.loginfo('enable callback')
self.dbw_enabled = msg.data
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
self.dbw_enabled = False
self.tl_detector_ready = False
self.target_velocity = 0.
self.target_yaw_dot = 0.
self.current_velocity = 0.
self.current_yaw_dot = 0.
self.last_update_time = None
# Initialize TwistController
self.twist_controller = TwistController(accel_limit, -1.,
max_steer_angle,
BrakeCmd.TORQUE_MAX,
wheel_base, steer_ratio,
max_lat_accel, max_steer_angle)
# Subscribe to topics
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd_callback)
rospy.Subscriber('/current_velocity', TwistStamped,
self.current_velocity_callback)
rospy.Subscriber('/vehicle/dbw_enabled', Bool,
self.dbw_enabled_callback)
rospy.Subscriber('/tl_detector_ready', Bool,
self.tl_detector_ready_callback)
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
# hold brake if tl_detector not ready
if not self.tl_detector_ready or self.last_update_time is None:
self.last_update_time = rospy.Time.now()
self.publish(0, BrakeCmd.TORQUE_MAX, 0)
rate.sleep()
continue
current_time = rospy.Time.now()
dt = current_time.to_sec() - self.last_update_time.to_sec()
self.last_update_time = current_time
if (dt > 0.075):
rospy.logwarn('slow DBW update, dt:%.3fs freq:%.1fhz', dt,
1. / dt)
# reset controller PIDs and skip calculations if DBW is off
if not self.dbw_enabled:
self.twist_controller.reset()
rate.sleep()
continue
accel, steer = self.twist_controller.control(
self.target_velocity, self.target_yaw_dot,
self.current_velocity, dt)
throttle = 0
brake = 0
if (accel < 0):
brake = -accel
else:
throttle = accel
# record data for debugging
# self.data_recorder(self.target_velocity, self.target_yaw_dot, throttle, brake, steer)
# rospy.loginfo('DBW a:%.3f y:%.3f', self.target_velocity, self.target_yaw_dot)
# rospy.loginfo('DBW t:%.3f b:%.3f s:%.3f', throttle, brake, steer)
self.publish(throttle, brake, steer)
rate.sleep()
def twist_cmd_callback(self, msg):
self.target_velocity = msg.twist.linear.x
self.target_yaw_dot = msg.twist.angular.z
# rospy.loginfo('twist_cmd: v:%.3f yd:%.3f', self.target_velocity, self.target_yaw_dot)
# log_twist_msg(msg, 'twist_cmd:')
def current_velocity_callback(self, msg):
self.current_velocity = msg.twist.linear.x
self.current_yaw_dot = msg.twist.angular.z
# rospy.loginfo('current_velocity: v:%.3f yd:%.3f', self.current_velocity, self.current_yaw_dot)
# log_twist_msg(msg, 'current_velocity:')
def dbw_enabled_callback(self, msg):
self.dbw_enabled = msg.data
# rospy.loginfo('dbw_enabled: %d', self.dbw_enabled)
def tl_detector_ready_callback(self, msg):
self.tl_detector_ready = msg.data
#rospy.loginfo('tl_detector_ready: %s', self.tl_detector_ready)
def publish(self, throttle, brake, steer):
#rospy.logwarn("In publish: %d; %d; %d;", 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 data_recorder(self, target_velocity, target_yaw_dot, throttle, brake,
steer):
dbw_data_filename = '/tmp/dbw_data.csv'
try:
foo = self.dbw_data
except:
self.dbw_data = []
with open(dbw_data_filename, 'wb') as csvfile:
rospy.loginfo('DBW data file: %s', csvfile.name)
csv_writer = csv.writer(csvfile, delimiter=',')
csv_writer.writerow([
'time', 't_speed', 't_yd', 'throttle', 'brake', 'steer',
'c_speed', 'c_yd'
])
time = rospy.Time.now().to_sec()
self.dbw_data.append([
time, target_velocity, target_yaw_dot, throttle, brake, steer,
self.current_velocity, self.current_yaw_dot
])
if len(self.dbw_data) == 1000:
with open(dbw_data_filename, 'ab') as csvfile:
csv_writer = csv.writer(csvfile, delimiter=',')
for row in self.dbw_data:
csv_writer.writerow(row)
rospy.loginfo('DBW data saved')
self.dbw_data = []
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
# Fetch value from parameter 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.)
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)
max_throttle_percentage = rospy.get_param('~max_throttle_percentage')
max_braking_percentage = rospy.get_param('~max_braking_percentage')
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,
'max_throttle_percentage': max_throttle_percentage,
'max_braking_percentage': max_braking_percentage
}
self.current_velocity = None
self.twist_cmd = None
self.dbw_enabled = False
self.controller = TwistController(**params)
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
# throttle, brake, steering = self.controller.control(<proposed linear and angular velocity>,
# <current linear and angular velocity>,
# <dbw status>,
# <any other argument you need>)
# if <dbw is enabled>:
# self.publish(throttle, brake, steer)
if self.current_velocity and self.twist_cmd:
# Adjust control commands according to target twist(linear/angular speed)
throttle, brake, steer = self.controller.control(
self.twist_cmd.twist, self.current_velocity.twist,
self.dbw_enabled)
if self.dbw_enabled:
self.publish(throttle, brake, steer)
rate.sleep()
def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def current_velocity_cb(self, msg):
self.current_velocity = msg
def twist_cmd_cb(self, msg):
self.twist_cmd = msg
rospy.loginfo(msg)
def dbw_enabled_cb(self, msg):
self.dbw_enabled = msg
def __init__(self):
rospy.init_node('dbw_node')
rospy.loginfo('abhishek - Starting 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
# specify the controller gain parameters
#1) Velocity controller
#self.velocity_control_params = [0.3, 0.01, 0.02]
self.velocity_control_params = [0.4, 0.0, 0.02]
#2 Yaw controller
yaw_controller = YawController(wheel_base, steer_ratio, ONE_MPH,
max_lat_accel, max_steer_angle)
total_vehicle_mass = vehicle_mass + fuel_capacity * GAS_DENSITY
# Initalize the TwistController object
self.controller = TwistController(self.velocity_control_params,
total_vehicle_mass, accel_limit,
decel_limit, wheel_radius,
yaw_controller)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/current_velocity',
TwistStamped,
self.cv_callback,
queue_size=1)
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_callback,
queue_size=1)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_callback,
queue_size=1)
self.dbw_enabled = False
# Init Twist values
self.twist_cmd_twist = TwistStamped().twist
self.current_velocity_twist = TwistStamped().twist
# self.desired_linear_vel = 0.0 # only x direction required
# self.desired_ang_vel = 0.0 # only z direction required
# self.actual_linear_vel = 0.0
# self.actual_ang_vel = 0.0
# Timers for the logitudinal controller. Not sure if we can rely on 50Hz rate.
self.prev_vel_msg_time = 0.0
# Specify the loop rate for the node
self.loop_rate = 50.0
self.loop()
class DBWNode(object):
def __init__(self):
rospy.init_node('dbw_node')
rospy.loginfo('abhishek - Starting 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
# specify the controller gain parameters
#1) Velocity controller
#self.velocity_control_params = [0.3, 0.01, 0.02]
self.velocity_control_params = [0.4, 0.0, 0.02]
#2 Yaw controller
yaw_controller = YawController(wheel_base, steer_ratio, ONE_MPH,
max_lat_accel, max_steer_angle)
total_vehicle_mass = vehicle_mass + fuel_capacity * GAS_DENSITY
# Initalize the TwistController object
self.controller = TwistController(self.velocity_control_params,
total_vehicle_mass, accel_limit,
decel_limit, wheel_radius,
yaw_controller)
# TODO: Subscribe to all the topics you need to
rospy.Subscriber('/current_velocity',
TwistStamped,
self.cv_callback,
queue_size=1)
rospy.Subscriber('/twist_cmd',
TwistStamped,
self.twist_callback,
queue_size=1)
rospy.Subscriber('/vehicle/dbw_enabled',
Bool,
self.dbw_callback,
queue_size=1)
self.dbw_enabled = False
# Init Twist values
self.twist_cmd_twist = TwistStamped().twist
self.current_velocity_twist = TwistStamped().twist
# self.desired_linear_vel = 0.0 # only x direction required
# self.desired_ang_vel = 0.0 # only z direction required
# self.actual_linear_vel = 0.0
# self.actual_ang_vel = 0.0
# Timers for the logitudinal controller. Not sure if we can rely on 50Hz rate.
self.prev_vel_msg_time = 0.0
# Specify the loop rate for the node
self.loop_rate = 50.0
self.loop()
def cv_callback(self, msg):
# Maybe it is to keep it ad twist object as we need to pass it to twist_controller for yaw_controller
self.current_velocity_twist = msg.twist
# self.actual_linear_vel = msg.twist.linear.x
# self.actual_ang_vel = msg.twist.angular.z
# rospy.loginfo('abhishek - cv_callback: act_vel: %s' % str(self.actual_linear_vel))
def twist_callback(self, msg):
# Maybe it is to keep it ad twist object as we need to pass it to twist_controller for yaw_controller
self.twist_cmd_twist = msg.twist
# self.desired_linear_vel = msg.twist.linear.x
# self.desired_ang_vel = msg.twist.angular.z
# rospy.loginfo('abhishek - twist_callback: desired_linear_vel: %s' % str(self.desired_linear_vel))
def dbw_callback(self, msg):
self.dbw_enabled = msg.data # data is a boolean value
rospy.loginfo('abhishek - dbw_received: %s' % self.dbw_enabled)
def loop(self):
rate = rospy.Rate(int(self.loop_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
current_time = rospy.get_time()
delta_time = current_time - self.prev_vel_msg_time
self.prev_vel_msg_time = current_time
# calculate the velocity error . in meter / sec
# velocity_error_mps = self.desired_linear_vel - self.actual_linear_vel
# Use dbw value to reset the controller states if needed.
throttle, brake, steering = self.controller.control(
self.dbw_enabled, self.twist_cmd_twist,
self.current_velocity_twist, delta_time)
# rospy.loginfo('dbw_enable: %s, vel_error: %s , throttle: %s \n '% (self.dbw_enabled, velocity_error_mps, throttle))
rospy.loginfo('throttle: %s, brake: %s, steering: %s \n ' %
(throttle, brake, steering))
if self.dbw_enabled:
self.publish(throttle, brake, steering)
# self.publish(1.0, 0.0, 0.0)
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)