예제 #1
0
 def __init__(self):
     print("Loading CAN DBC file...")
     self.can_msg_parser = load_dbc_file(
         'honda_civic_touring_2016_can_for_cantools.dbc')
     print("Connecting to Panda board...")
     self.panda = Panda()
     print("Ready.")
    def __init__(self):
        rp = RosPack()
        pkgpath = rp.get_path('panda_bridge_ros')
        pkgpath += '/config/honda_civic_touring_2016_can_for_cantools.dbc'
        self.parser = load_dbc_file(pkgpath)
        self.p = Panda()
        self.p.set_safety_mode(self.p.SAFETY_ALLOUTPUT)
        self.gas_val = 0
        self.brake_val = 0
        self.steer_val = 0
        self.cruise_modifier = 0.0
        self.sub_engine = rospy.Subscriber('/joy',
                                           Joy,
                                           self.engine_cb,
                                           queue_size=1)
        self.sub_steer = rospy.Subscriber('/steering_amount',
                                          Int16,
                                          self.steer_cb,
                                          queue_size=1)
        self.sub_brake = rospy.Subscriber('/braking_amount',
                                          Int16,
                                          self.brake_cb,
                                          queue_size=1)
        self.ddr = DDynamicReconfigure("drive_car")
        self.ddr.add_variable(
            "rate_buttons",
            "rate as to which send the button presses for cruise control", 1,
            1, 100)

        self.add_variables_to_self()

        self.ddr.start(self.dyn_rec_callback)
예제 #3
0
 def __init__(self, dbc=False):
     panda_list = Panda.list()
     # choose panda serial prot
     if len(panda_list) > 1:
         for i, s in enumerate(panda_list, 1):
             print('{}) {}'.format(i, s))
         serial = panda_list[input('Please input 1, 2,.... or 10 number: ')
                             - 1]
     else:
         serial = panda_list[0]
     # Connect to panda
     if serial in panda_list:
         self.panda = Panda(serial)
         self.panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
         self.panda.can_clear(0)
         self.frame = 0
         print('Connect Panda [Send]')
     else:
         print('Not Panda connect')
         exit()
     # add dbc decoder
     if dbc:
         self.can_msg_parser = load_dbc_file(dbc)
         print(self.can_msg_parser.messages)
     self.upload_data = OrderedDict(
         (('wheel_angle', 0), ('SPEED', 0), ('gear_1', 35), ('gear_2', 96),
          ('brake_LI', 0), ('brake_H', 0), ('handcrat', 0),
          ('LIGHT_SMALL', 0), ('LIGHT_BIG', 0), ('LIGHT_FLASH',
                                                 0), ('door_lock', 128)))
예제 #4
0
 def __init__(self):
     self.can_msg_parser = load_dbc_file(
         'honda_civic_touring_2016_can_for_cantools.dbc')
     self.pub = rospy.Publisher('can_frame_msgs_human_friendly',
                                String,
                                queue_size=10)
     self.pub = rospy.Publisher('can_frame_msgs_human_friendly',
                                String,
                                queue_size=10)
     self.frame_sub = rospy.Subscriber('can_frame_msgs',
                                       Frame,
                                       self._cb,
                                       queue_size=10)
 def __init__(self):
     rp = RosPack()
     pkgpath = rp.get_path('panda_bridge_ros')
     pkgpath += '/config/honda_civic_touring_2016_can_for_cantools.dbc'
     self.parser = load_dbc_file(pkgpath)
     self.p = Panda()
     self.p.set_safety_mode(self.p.SAFETY_ALLOUTPUT)
     self.gas_val = 0
     self.brake_val = 0
     self.steer_val = 0
     self.engine_modifier = 0.0
     self.sub_engine = rospy.Subscriber('/joy',
                                        Joy,
                                        self.engine_cb,
                                        queue_size=1)
     self.sub_steer = rospy.Subscriber('/steering_amount',
                                       Int16,
                                       self.steer_cb,
                                       queue_size=1)
     self.sub_brake = rospy.Subscriber('/braking_amount',
                                       Int16,
                                       self.brake_cb,
                                       queue_size=1)
예제 #6
0
    def __init__(self):
        rp = RosPack()
        pkg_path = rp.get_path('panda_bridge_ros')
        self.can_msg_parser = load_dbc_file(pkg_path + '/config/' +
                                            'honda_civic_touring_2016_can_for_cantools.dbc')
        self.pub = rospy.Publisher('can_frame_msgs_human_friendly', String,
                                   queue_size=10)
        self.frame_sub = rospy.Subscriber('can_frame_msgs', Frame,
                                          self._cb, queue_size=10)
        self.msg_id_to_msg_name = {}
        understood_msgs = []
        for msg in self.can_msg_parser.messages:
            this_msg = {}
            this_msg['frame_id'] = msg.frame_id
            this_msg['name'] = msg.name
            this_msg['signals'] = msg.signals
            this_msg['nodes'] = msg.nodes
            understood_msgs.append(this_msg)
            self.msg_id_to_msg_name[msg.frame_id] = msg.name

        rospy.loginfo("We can interpret the messages:")
        pp = PrettyPrinter()
        rospy.loginfo(pp.pformat(understood_msgs))
예제 #7
0
    #             signal('CHIME', 47, 3, 'big_endian', False, 1, 0, 0, 7, 'None', False, None, {4: 'double_chime', 3: 'single_chime', 2: 'continuous_chime', 1: 'repeating_chime', 0: 'no_chime'}, None),
    #             signal('ZEROS_BOH6', 44, 1, 'big_endian', False, 1, 0, 0, 1, 'None', False, None, None, None),
    #             signal('FCW', 43, 2, 'big_endian', False, 1, 0, 0, 3, 'None', False, None, {3: 'fcw', 2: 'fcw', 1: 'fcw', 0: 'no_fcw'}, None),
    #             signal('ZEROS_BOH4', 55, 8, 'big_endian', False, 1, 0, 0, 0, 'None', False, None, None, None),
    #             signal('COUNTER', 61, 2, 'big_endian', False, 1, 0, 0, 3, 'None', False, None, None, None),
    #             signal('CHECKSUM', 59, 4, 'big_endian', False, 1, 0, 0, 15, 'None', False, None, None, None)]}

    #                    apply_brake, pcm_override, pcm_cancel_cmd, chime, idx
    # idx refers to the COUNTER field we see
    brake_cmd = create_brake_command(True, True, True, 0, 0)
    # Looks like: [506, 0, '\x00A\x13\x80\x80\x00\x00\x05', 0]

    rp = RosPack()
    pkg_path = rp.get_path('panda_bridge_ros')
    can_msg_parser = load_dbc_file(
        pkg_path + '/config/' +
        'honda_civic_touring_2016_can_for_cantools.dbc')

    # Let's double check the brake_cmd
    decoded_brake_cmd = can_msg_parser.decode_message(brake_cmd[0],
                                                      brake_cmd[2])
    # {'BRAKE_LIGHTS': 1,
    # 'CHECKSUM': 5,
    # 'CHIME': 'no_chime',
    # 'COMPUTER_BRAKE': 1,
    # 'COMPUTER_BRAKE_REQUEST': 1,
    # 'COMPUTER_BRAKE_REQUEST_2': 1,
    # 'COUNTER': 0,
    # 'CRUISE_BOH2': 0,
    # 'CRUISE_BOH3': 0,
    # 'CRUISE_CANCEL_CMD': 1,