예제 #1
0
    def publish(self, throttle, brake, steer):
        rospy.loginfo(
            'DBWNode::publish - publishing throttle (%s), brake (%s) and steer (%s) values twist_cmd x %s z %s', \
            throttle, brake, steer, self.twist_cmd.twist.linear.x, self.twist_cmd.twist.angular.z)
        tcmd = ThrottleCmd()
        tcmd.enable = True
        tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
        tcmd.pedal_cmd = throttle
        # rospy.loginfo('DBWNode::publish - publishing throttle command')
        if brake == 0.0 and throttle > 0.0:
            self.throttle_pub.publish(tcmd)

        scmd = SteeringCmd()
        scmd.enable = True
        scmd.steering_wheel_angle_cmd = steer
        # rospy.loginfo('DBWNode::publish - publishing steering command')
        self.steer_pub.publish(scmd)

        bcmd = BrakeCmd()
        bcmd.enable = True
        bcmd.boo_cmd = True
        bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
        bcmd.pedal_cmd = brake
        # rospy.loginfo('DBWNode::publish - publishing brake command')
        if brake != 0.0:
            self.brake_pub.publish(bcmd)
예제 #2
0
    def publish(self, throttle, brake, steer):

        # Take care not to publish both throttle and brake
        if throttle > 0:
            tcmd = ThrottleCmd()
            tcmd.enable = True
            tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
            tcmd.pedal_cmd = throttle
            self.throttle_pub.publish(tcmd)

        if self.speed > 0.1:
            scmd = SteeringCmd()
            scmd.enable = True
            scmd.steering_wheel_angle_cmd = steer
            self.steer_pub.publish(scmd)
            rospy.loginfo('steer cmd: %s', steer)

        if brake > 0:
            bcmd = BrakeCmd()
            bcmd.enable = True
            bcmd.pedal_cmd_type = BrakeCmd.CMD_PERCENT
            # It's uncertain what these units should be. It looks like
            # CMD_PERCENT is 0-1 for ThrottleCmd but 0-100 for BrakeCmd
            # Further it looks like 100% is not a lot of braking at all
            # So I've doubled that amount here to have a moderate amount
            # of braking as observed in the simulator
            bcmd.pedal_cmd = brake * 200
            bcmd.boo_cmd = True
            self.brake_pub.publish(bcmd)
예제 #3
0
def brake_on_off(brake_on):
    ''' Brake

    # Brake pedal
    # Options defined below
    float32 pedal_cmd
    uint8 pedal_cmd_type
    
    # Brake On Off (BOO), brake lights
    bool boo_cmd
    
    # Enable
    bool enable
    
    # Ignore driver overrides
    bool ignore
    
    # Watchdog counter (optional)
    uint8 count
    
    uint8 CMD_NONE=0
    uint8 CMD_PEDAL=1   # Unitless, range 0.15 to 0.50
    uint8 CMD_PERCENT=2 # Percent of maximum torque, range 0 to 1
    uint8 CMD_TORQUE=3  # Nm, range 0 to 3250
    
    float32 TORQUE_BOO=520  # Nm, brake lights threshold
    float32 TORQUE_MAX=3412 # Nm, maximum torque
    
    '''
    if not brake_on:
        brake_msg = BrakeCmd()
        brake_msg.pedal_cmd_type = 1
        brake_msg.pedal_cmd = 0.15
        brake_msg.boo_cmd = False
        brake_msg.enable = True
        brake_on = False
    else:
        brake_msg = BrakeCmd()
        brake_msg.pedal_cmd_type = 1
        brake_msg.pedal_cmd = 0.5
        brake_msg.boo_cmd = True
        brake_msg.enable = True
        brake_on = True
        
    brake_pub.publish(brake_msg)
    print("Brake on " + str(brake_msg))
예제 #4
0
 def timer_cmd(self, event):
     if self.brake_cmd > 0:
         msg = BrakeCmd()
         msg.enable = True
         msg.pedal_cmd_type = BrakeCmd.CMD_PEDAL
         msg.pedal_cmd = self.brake_cmd
         if msg.pedal_cmd > 0.2:
             msg.boo_cmd = True
         self.pub.publish(msg)
예제 #5
0
 def publishBrakeCmd(self, pedalVal):
     msg = BrakeCmd()
     msg.pedal_cmd = pedalVal
     msg.pedal_cmd_type = 2
     msg.boo_cmd = False
     msg.enable = True
     msg.clear = False
     msg.ignore = False
     msg.count = 0
     self.brake_pub.publish(msg)
예제 #6
0
def make_brake_msg(brake_value=0.0):

    brake_command_object = BrakeCmd()
    brake_command_object.pedal_cmd = brake_value
    brake_command_object.pedal_cmd_type = 2
    brake_command_object.boo_cmd = False
    brake_command_object.enable = False
    brake_command_object.ignore = False
    brake_command_object.count = 0

    return brake_command_object
예제 #7
0
    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()
예제 #8
0
    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>,
            if self.dbw_enabled:
                throttle, deceleration, steering = self.twist_controller.control(
                    self.current_velocity, self.desired_long_velocity,
                    self.desired_angular_velocity)
                # 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()
예제 #9
0
    def publish_brake(self, brake_torque):

        assert (0.0 <= brake_torque <= BrakeCmd.TORQUE_MAX)

        if self.brake.update(brake_torque):
            cmd = BrakeCmd()
            cmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
            cmd.pedal_cmd = brake_torque
            cmd.boo_cmd = True
            cmd.enable = True

            #rospy.logerr('brake pedal_cmd is %f, enable %d', brake_torque, cmd.enable)

            self.brake_pub.publish(cmd)
예제 #10
0
	def brake_cmd_publisher(self, brake_value=0.0):
		rospy.get_time()
		brake_command_object = BrakeCmd()
		brake_command_object.pedal_cmd = brake_value
		brake_command_object.pedal_cmd_type = 1
		brake_command_object.boo_cmd = True
		brake_command_object.enable = True
		brake_command_object.ignore = False
		brake_command_object.count = 0
		rospy.loginfo("Brake Publish ::>"+str(brake_command_object.pedal_cmd))
		rospy.loginfo("Brake Pedal_cmd Report ::>"+str(self.brake_pedal_cmd))
		rospy.loginfo("Brake Pedal_input Report ::>"+str(self.brake_pedal_input))
		rospy.loginfo("Brake Pedal_output Report ::>"+str(self.brake_pedal_output))
		self.pub_brake_cmd.publish(brake_command_object)
	def publishBrakeCmd(self, pedalVal):
		msg = BrakeCmd()
		msg.pedal_cmd = pedalVal
		msg.pedal_cmd_type = 2
		'''
		pedal_cmd_type=1: Unitless, range 0.15 to 0.50
					  =2: Percent of maximum torque, range 0 to 1
					  =3: Torque (unit: N.m), range 0 to 3250
		'''
		msg.boo_cmd = False
		msg.enable = True
		msg.clear = False
		msg.ignore = False
		msg.count = 0
		self.brake_pub.publish(msg)
예제 #12
0
 def brake_cmd_publisher(self, brake_value=0.0):
     """
     ID = 96 in BusCan
     # Brake pedal
     # Options defined below
     float32 pedal_cmd
     uint8 pedal_cmd_type
     
     # Brake On Off (BOO), brake lights
     bool boo_cmd
     
     # Enable
     bool enable
     
     # Ignore driver overrides
     bool ignore
     
     # Watchdog counter (optional)
     uint8 count
     
     uint8 CMD_NONE=0
     uint8 CMD_PEDAL=1   # Unitless, range 0.15 to 0.50
     uint8 CMD_PERCENT=2 # Percent of maximum torque, range 0 to 1
     uint8 CMD_TORQUE=3  # Nm, range 0 to 3250
     
     float32 TORQUE_BOO=520  # Nm, brake lights threshold
     float32 TORQUE_MAX=3412 # Nm, maximum torque
     """
     #msg_str = "Publishing Brake message in /vehicle/brake_cmd %s" % rospy.get_time()
     #rospy.loginfo(msg_str)
     brake_command_object = BrakeCmd()
     #brake_command_object.pedal_cmd = random.uniform(0.15, 0.5)
     brake_command_object.pedal_cmd = brake_value
     brake_command_object.pedal_cmd_type = 1
     brake_command_object.boo_cmd = False
     brake_command_object.enable = False
     brake_command_object.ignore = False
     brake_command_object.count = 0
     rospy.loginfo("Brake Publish ::>" +
                   str(brake_command_object.pedal_cmd))
     self.pub_brake_cmd.publish(brake_command_object)