def run(self): self.panda.set_safety_mode(self.panda.SAFETY_HONDA) try: while True: data = self.panda.can_recv() for msg in data: decoded_msg = None try: decoded_msg = self.decode_msg(msg) except KeyError: pass except ValueError: pass if decoded_msg: # print decoded_msg # continue for k in decoded_msg.keys(): if 'STEER_TORQUE' in k: print decoded_msg print msg print "---" steer_msg = create_steering_control(1000, 0)[0] print steer_msg print self.decode_msg(steer_msg) print self.panda.can_send(steer_msg[0], steer_msg[2], 0) # msg_to_send = self.encode_msg(891, {'CHECKSUM': 15, 'COUNTER': 1, 'WIPERS': 1}) # self.panda.can_send(msg[0], '\x00\x00\x01 \x00\x00\x00\x00', 0) # for pending_msg in self.pending_msg: # panda.can_send(0xXXXX, pending_msg, 0) except KeyboardInterrupt: self.panda.close()
def run(self): print("Publishing...") idx_counter = 0 total_cmds_sent = 0 while not rospy.is_shutdown(): cmd = create_gas_command(self.gas_val, idx_counter) print("command is: " + str(cmd)) print("Sending: " + str(cmd) + " (#" + str(total_cmds_sent) + ") Gas val: " + str(self.gas_val)) self.p.can_send(cmd[0], cmd[2], 0) cmd = create_steering_control(self.steer_val, idx_counter)[0] print("Sending: " + str(cmd) + " (#" + str(total_cmds_sent) + ") steer val: " + str(self.steer_val)) self.p.can_send(cmd[0], cmd[2], 0) # (apply_brake, pcm_override, pcm_cancel_cmd, chime, idx): cmd = create_brake_command(self.brake_val, 1, 0, 0, idx_counter) print("Sending: " + str(cmd) + " (#" + str(total_cmds_sent) + ") brake val: " + str(self.brake_val)) self.p.can_send(cmd[0], cmd[2], 0) idx_counter += 1 idx_counter %= 4 total_cmds_sent += 1 time.sleep(0.01)
def run(self): print("Publishing...") idx_counter = 0 total_cmds_sent = 0 while not rospy.is_shutdown(): cmd = create_steering_control(self.steer_val, idx_counter)[0] print("Sending: " + str(cmd) + " (#" + str(total_cmds_sent) + ") steer val: " + str(self.steer_val)) self.p.can_send(cmd[0], cmd[2], 0) idx_counter += 1 idx_counter %= 4 total_cmds_sent += 1 time.sleep(0.01)
def run(self): print("Publishing...") idx_counter = 0 total_cmds_sent = 0 iterations = 0 last_rate = time.time() while not rospy.is_shutdown(): #print("rate: " + str(self.rate_buttons)) if iterations % (100 / self.rate_buttons) == 0: if self.cruise_modifier != 0.0: if self.cruise_modifier > 0.0: cruise_command = 'accel_res' elif self.cruise_modifier < 0.0: cruise_command = 'decel_set' settings_command = 'none' print("Sending cruise command: " + cruise_command) # (xmission_speed, engine_rpm=2000, odometer=3, idx=0): cmd = create_buttons_command(cruise_command, settings_command, idx_counter) print("command is: " + str(cmd)) print("Sending: " + str(cmd) + " (#" + str(total_cmds_sent) + ") engine modifier val: " + str(self.cruise_modifier)) self.p.can_send(cmd[0], cmd[2], 1) cmd = create_steering_control(self.steer_val, idx_counter)[0] # print("Sending: " + str(cmd) + # " (#" + str(total_cmds_sent) + # ") steer val: " + str(self.steer_val)) self.p.can_send(cmd[0], cmd[2], 1) # (apply_brake, pcm_override, pcm_cancel_cmd, chime, idx): cmd = create_brake_command(self.brake_val, 1, 0, 0, idx_counter) # print("Sending: " + str(cmd) + # " (#" + str(total_cmds_sent) + # ") brake val: " + str(self.brake_val)) self.p.can_send(cmd[0], cmd[2], 1) idx_counter += 1 idx_counter %= 4 # its wrong, but who cares total_cmds_sent += 1 time.sleep(0.01) iterations += 1
def run(self): print("Publishing...") idx_counter = 0 idx_counter_engine = 0 total_cmds_sent = 0 last_received_xmission_speed = 0 last_received_odometer = 0 last_received_xmission_speed_2 = 0 last_engine_rpm = 0 iterations = 0 while not rospy.is_shutdown(): # receive first block = self.p.can_recv() for msg in block: # if its an engine message if msg[0] == 344 and msg[3] == 0: fields = self.parser.decode_message(msg[0], msg[2]) # print("we read engine message with speed:") # print(fields['XMISSION_SPEED']) last_received_xmission_speed = fields['XMISSION_SPEED'] last_received_odometer = fields['ODOMETER'] last_received_xmission_speed_2 = fields['XMISSION_SPEED2'] last_engine_rpm = fields['ENGINE_RPM'] break # Engine rpm stuff if self.engine_modifier != 0.0: # we need to invert the modifier signal # cause we are tricking the cruise control to go at the speed we want # so we need to create a engine data message that ssays # we are driving slower than we should in order for it to accelerate # limit to +- 15kmph the max acceleration requested speed = last_received_xmission_speed + \ (15.0 * self.engine_modifier * -1.0) print("latest_received_xmission_speed: " + str(last_received_xmission_speed)) print("we modify by: " + str((15.0 * self.engine_modifier * -1.0))) print("which results in: " + str(speed)) # (xmission_speed, engine_rpm=2000, odometer=3, idx=0): cmd = create_engine_data(speed, last_engine_rpm, last_received_odometer, idx_counter_engine) idx_counter_engine += 1 idx_counter_engine %= 4 print("command is: " + str(cmd)) print("Sending: " + str(cmd) + " (#" + str(total_cmds_sent) + ") engine modifier val: " + str(self.engine_modifier)) print("speed:" + str(speed) + " rpm: " + str(last_engine_rpm)) self.p.can_send(cmd[0], cmd[2], 0) if iterations % 5 == 0: cmd = create_steering_control(self.steer_val, idx_counter)[0] # print("Sending: " + str(cmd) + # " (#" + str(total_cmds_sent) + # ") steer val: " + str(self.steer_val)) self.p.can_send(cmd[0], cmd[2], 1) # (apply_brake, pcm_override, pcm_cancel_cmd, chime, idx): cmd = create_brake_command(self.brake_val, 1, 0, 0, idx_counter) # print("Sending: " + str(cmd) + # " (#" + str(total_cmds_sent) + # ") brake val: " + str(self.brake_val)) self.p.can_send(cmd[0], cmd[2], 1) idx_counter += 1 idx_counter %= 4 # its wrong, but who cares total_cmds_sent += 1 time.sleep(0.002) iterations += 1
from accum_msgs_tools import replace_messages_by_id from cPickle import load, dump import sys from honda_create_msgs import create_steering_control if __name__ == '__main__': if len(sys.argv) < 2: print("Usage: ") print(sys.argv[0] + " accum.pickle") exit(0) pickle_accum_file = sys.argv[1] pickle_timestamps_file = pickle_accum_file.replace('.pickle', '_timestamps.pickle') frame_id = 228 _, _, new_message, _ = create_steering_control(-3840, 0)[0] print("Replacing all messages with frame_id: " + str(frame_id)) print("To have data field '" + str(new_message) + "'") print len(new_message) accum = load(open(pickle_accum_file, 'r')) timestamps = load(open(pickle_timestamps_file, 'r')) replaced_accum = replace_messages_by_id(frame_id, accum, new_message) replaced_accum_file = pickle_accum_file.replace('.pickle', '_replaced.pickle') replaced_accum_timestamp_file = pickle_timestamps_file.replace( '_timestamps.pickle', '_replaced_timestamps.pickle') print("Writing to " + replaced_accum_file + " and " + replaced_accum_timestamp_file)
panda.set_safety_mode(panda.SAFETY_ALLOUTPUT) # What we know about steering: # {'frame_id': 228, # 'name': 'STEERING_CONTROL', # 'nodes': ['ADAS'], # 'signals': [signal('STEER_TORQUE', 7, 16, 'big_endian', True, 1, 0, -3840, 3840, 'None', False, None, None, None), # signal('STEER_TORQUE_REQUEST', 23, 1, 'big_endian', False, 1, 0, 0, 1, 'None', False, None, None, None), # signal('SET_ME_X00', 22, 7, 'big_endian', False, 1, 0, 0, 127, 'None', False, None, None, None), # signal('SET_ME_X00_2', 31, 8, 'big_endian', False, 1, 0, 0, 0, 'None', False, None, None, None), # signal('COUNTER', 37, 2, 'big_endian', False, 1, 0, 0, 3, 'None', False, None, None, None), # signal('CHECKSUM', 35, 4, 'big_endian', False, 1, 0, 0, 15, 'None', False, None, None, None)]}, # apply_steer, idx # idx refers to the COUNTER field we see steer_cmd = create_steering_control(3840, 0)[0] # print steer_cmd # Looks like: [228, 0, '\x0f\x00\x80\x00\x0f', 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 steer_cmd decoded_steer_cmd = can_msg_parser.decode_message(steer_cmd[0], steer_cmd[2]) # {'CHECKSUM': 15, 'COUNTER': 0, 'STEER_TORQUE': 3840, 'SET_ME_X00': 0, 'STEER_TORQUE_REQUEST': 1, 'SET_ME_X00_2': 0} print("Press Enter to send steering commands, enter a number to change the steer value")