class DBWNode(object): """ **Diagram (6) - New commands for car actuators - CONTROL** When Self-Driving Mode activated, this node will then actuate throttle, brake and steering to navigate the car towards target trajectory and velocity. """ def __init__(self): rospy.init_node('dbw_node') # variables self.current_ego_pose = None # ego car current position and orientation # ROS Server Parameters 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) self.wheel_base = rospy.get_param('~wheel_base', 2.8498) self.steer_ratio = rospy.get_param('~steer_ratio', 14.8) self.max_lat_accel = rospy.get_param('~max_lat_accel', 3.) self.max_steer_angle = rospy.get_param('~max_steer_angle', 8.) self.max_throttle = rospy.get_param('~max_throttle_proportional', 0.8) self.max_brake = rospy.get_param('~max_brake_proportional', -0.8) # Publishers self.steer_pub = rospy.Publisher('/vehicle/steering_cmd', SteeringCmd, queue_size=1) self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd', ThrottleCmd, queue_size=1) self.brake_pub = rospy.Publisher('/vehicle/brake_cmd', BrakeCmd, queue_size=1) # Subscribers # TODO: Subscribe to all the topics you need to rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cb, queue_size=1) rospy.Subscriber('/current_velocity', TwistStamped, self.velocity_cb, queue_size=1) rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb, queue_size=1) rospy.Subscriber('/final_waypoints', Lane, self.waypoints_cb, queue_size=1) rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb, queue_size=1) # Subscribed messages self.twist_cmd = None self.twist_cmd_linear_velocity = None self.twist_cmd_angular_velocity = None self.velocity = None self.current_linear_velocity = None self.current_angular_velocity = None self.dbw_enabled = False self.waypoints = None # TODO: Create `TwistController` object self.controller = Controller(self.wheel_base, self.steer_ratio, self.max_lat_accel, self.max_steer_angle) self.loop() def loop(self): """ Gets predicted throttle, brake, and steering using TwistController. """ # You should only publish the control commands if dbw is enabled rate = rospy.Rate(50) # 50Hz recommended by John Chen while not rospy.is_shutdown(): data = [self.velocity, self.waypoints, self.current_ego_pose] all_available = all([x is not None for x in data]) if not all_available: continue if len(self.waypoints) >= POINTS_TO_FIT: # print("target_velocity aka self.waypoints[0].twist.twist.linear.x : ", self.waypoints[0].twist.twist.linear.x) # e.g. 11.1112 # target_velocity = self.waypoints[0].twist.twist.linear.x target_velocity = self.twist_cmd_linear_velocity # print("current_linear_velocity aka self.velocity.linear.x : ", self.velocity.linear.x) # e.g. 0.267761447712 # current_linear_velocity = self.velocity.linear.x current_linear_velocity = self.current_linear_velocity # Get corrected steering using twist_controller # cte = self.cte_calc(self.current_ego_pose, self.waypoints) cte = self.twist_cmd_angular_velocity steer = self.controller.control(cte, self.dbw_enabled, self.twist_cmd_linear_velocity, self.twist_cmd_angular_velocity, current_linear_velocity) # steer = cte * self.steer_ratio # print("twist_cmd_angular_velocity aka self.twist_cmd.twist.angular.z : ", self.twist_cmd.twist.angular.z) # throttle, brake = self.controller.control_speed_based_on_torque(target_velocity, # current_linear_velocity, # 0.5, # self.vehicle_mass, # self.wheel_radius) throttle, brake = self.controller.control_velocity_based_on_proportional_throttle_brake( target_velocity, current_linear_velocity, self.max_throttle, self.max_brake) else: # not enough waypoints so publish heavy break rospy.loginfo("Number of waypoint received is : %s", len(self.waypoints)) throttle, brake, steer = 0, 200, 0 if self.dbw_enabled: self.publish(throttle, brake, steer) rospy.loginfo("published throttle : %s, brake : %s, steer : %s", throttle, brake, steer) # rospy.logwarn("throttle %s, brake %s, steer %s adjustments by dbw_node", # 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 pose_cb(self, message): self.current_ego_pose = message.pose def twist_cb(self, message): self.twist_cmd = message self.twist_cmd_linear_velocity = self.twist_cmd.twist.linear.x self.twist_cmd_angular_velocity = self.twist_cmd.twist.angular.z def velocity_cb(self, message): self.velocity = message.twist self.current_linear_velocity = message.twist.linear.x self.current_angular_velocity = message.twist.angular.z def waypoints_cb(self, message): self.waypoints = message.waypoints def dbw_enabled_cb(self, message): """ Enabled Self-Driving mode will publish throttle, brake and steer values. """ self.dbw_enabled = bool(message.data) if self.dbw_enabled: rospy.logwarn("*** ============================= ***") rospy.logwarn("*** Self-Driving mode activated ! ***") rospy.logwarn("*** ============================= ***") else: rospy.logwarn("*** =============================== ***") rospy.logwarn("*** Manual Driving mode activated ! ***") rospy.logwarn("*** =============================== ***")