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 publish(self, Throttle, BreakPress, SteeringAngle): 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 = SteeringAngle self.steer_pub.publish(scmd) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = BreakPress self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer): #rospy.logwarn("Throttle: {0}, Brake: {1}, Steer: {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 publish(self, throttle, brake, steer): # rospy.logwarn("PUBLISH: throttle={:.2f}, brake={:.2f}, steering={:.2f}, dbw_enabled={:}".format(throttle, brake, steer, self.dbw_enabled)) 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 publish(self, throttle, brake, steer): "publish throttle, brake and steer commands, each to their corrresponding topics" 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 publish(self, throttle, brake, steer): ## Function to publish the dbw commands, based on args 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 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 #rospy.loginfo('TwistController: Steering = ' + str(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 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) # do not send brake command unless required if brake > 0.001: bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer): # rospy.loginfo("throttle: {0}, brake {1}, steer {2}, linear {3}, angular {4}".format(throttle, brake, steer, self.linear_vel, self.angular_vel)) 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 publish(self, throttle, brake, steer): # The numerical issue is taken care by brake_deadband if brake > 0: bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_PERCENT 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)
def publish(self, throttle, brake, steer): # Make sure not so brake and throttle at the same time 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)
def publish(self, throttle, brake, steer): rospy.logdebug('publish with values throttle=%s, steer=%s , brake=,%s', throttle, steer, brake) 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 publish(self, throttle, brake, steer): rospy.loginfo("THROTTLE: {}, BRAKE: {}, STEER: {}\n".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 publish(self, throttle, brake, steer): tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle self.throttle_pub.publish(tcmd) scmd = SteeringCmd() scmd.enable = True scmd.steering_wheel_angle_cmd = steer self.steer_pub.publish(scmd) bcmd = BrakeCmd() bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE # We want to command the brake in torque Nm # If the requested torque is over the threshold power on the breake lights bcmd.enable = brake > BrakeCmd.TORQUE_BOO bcmd.pedal_cmd = brake # Set the brake value self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer): # This function publishes throttle, brake, and steering # Throttle values passed to publish should be in the range 0 to 1 # Brake values passed to publish should be in units of torque (N*m) 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 publish(self, throttle, brake, steer): ''' Function to convert values from controller to ROS messages ''' 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 publish(self, throttle, brake, steer): rospy.loginfo( 'DBWNode: Publisher started with: throttle -> %.2f brake -> %.2f steer -> %.2f', 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 publish(self, throttle, brake, steer): tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle rospy.logdebug('Throttle: {}'.format(throttle)) self.throttle_pub.publish(tcmd) scmd = SteeringCmd() scmd.enable = True scmd.steering_wheel_angle_cmd = steer rospy.logdebug('Steer: {}'.format(steer)) self.steer_pub.publish(scmd) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake rospy.logdebug('Brake: {}'.format(brake)) self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer): # Throttle values passed to publish are in the range 0 to 1. # A throttle of 1 means the vehicle throttle will be fully engaged. 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 values passed to publish are in units of torque (N*m). See twist_controller for the math. bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer): """ Publish the throttle, brake, and steer command to the car while the DBW system is enabled """ 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 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) rospy.loginfo("Velocity error: %s", self.target_long_vel - self.cur_long_vel)
def publish(self, throttle, brake, steer): rospy.loginfo( 'publishing dbw controller output : throttle %f, brake %f, steer %f', 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 publish(self, throttle, brake, steer): # Loggging ... rospy.loginfo("CRTL T : %.2f, B : %.2f, S : %.2f", 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 publish(self, throttle, brake, steer): tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle #rospy.loginfo('Published throttle command: %f'%(throttle)) self.throttle_pub.publish(tcmd) scmd = SteeringCmd() scmd.enable = True scmd.steering_wheel_angle_cmd = steer #rospy.logwarn('Published steering command: %f'%(steer)) self.steer_pub.publish(scmd) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake #rospy.logerr('Published brake command: %f'%(brake)) self.brake_pub.publish(bcmd)
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): rospy.loginfo("publishing throttle %f, brake %f, steer %f", throttle, brake, steer) # We'll either publish the throttle or brake, not both if throttle != 0: tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle self.throttle_pub.publish(tcmd) else: bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) scmd = SteeringCmd() scmd.enable = True scmd.steering_wheel_angle_cmd = steer self.steer_pub.publish(scmd)
def publish(self, throttle, brake, steer): if DEBUG: rospy.loginfo("Controls: %.15f:%.15f:%.15f" % (throttle, brake, steer)) tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle self.throttle_pub.publish(tcmd) scmd = SteeringCmd() scmd.enable = True scmd.steering_wheel_angle_cmd = steer self.steer_pub.publish(scmd) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer): #debug_msg = "DBW T:{} \t B:{} \t S:{}\t".format(throttle, brake, steer) #rospy.logdebug(debug_msg) self.count += 1 tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle self.throttle_pub.publish(tcmd) scmd = SteeringCmd() scmd.enable = True scmd.steering_wheel_angle_cmd = steer self.steer_pub.publish(scmd) bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer): #Normally a car is driven with either the gas or the brake pedal pressed. This way, the throttle command with steering will be sent OR the brake command with the steering. Never throttle and brake at the same time if throttle != 0: tcmd = ThrottleCmd() tcmd.enable = True tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT tcmd.pedal_cmd = throttle self.throttle_pub.publish(tcmd) #rospy.loginfo("Throttle %f and Steering %f being published...", throttle, steer) else: bcmd = BrakeCmd() bcmd.enable = True bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE bcmd.pedal_cmd = brake self.brake_pub.publish(bcmd) #rospy.loginfo("Brake %f and Steering %f being published...", throttle, steer) scmd = SteeringCmd() scmd.enable = True scmd.steering_wheel_angle_cmd = steer self.steer_pub.publish(scmd)
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)