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")