def main(args=None):
#FUUUUUUUUUUUUUUUU
# Finaly. with rospy, you need to include the namespace with the service, if there is any. unlike C++, that does it under the hood.
# Hoenstly Just stick with C++. THeres so much more information out there.
rospy.wait_for_service('Send_Data_To_World')
try:
Send_Data_To_World = rospy.ServiceProxy('Send_Data_To_World',
Monitor_Send_Data_To_World)
can = Frame()
can.header.seq = 1
can.header.frame_id = "22"
can.id = 1 # trident's message ID is 1
can.is_error = False
can.is_rtr = False
can.is_extended = False
can.dlc = 2
can.data = [0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11]
letter = Monitor_Jetson_To_World()
letter.message_name.data = "protocol_MID_TRIDENT_deviceName"
letter.CAN_MSG = can
resp = Send_Data_To_World(letter)
print(resp.output.data)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
def can_talker(self):
if (self.steering_cmd is not None and self.speed_cmd is not None
and self.dbw_enabled is not None):
msg = Frame()
command_msg = self.can_db.get_message_by_name('Commands')
msg.id = command_msg.frame_id
msg.dlc = 8
# Manual can encoding
steering_cmd_raw = np.uint16(
(self.steering_cmd + 81.92) /
0.005) #TODO do not hardcode constants
steering_cmd_1 = int(np.uint8(np.uint16(steering_cmd_raw) >> 8))
steering_cmd_2 = int(np.uint8(steering_cmd_raw))
checksum = np.uint8(self.speed_cmd + steering_cmd_1 +
steering_cmd_2 + self.gear_cmd +
self.dummy_constant * 3)
data_list = [
int(np.uint8(self.speed_cmd)), steering_cmd_1, steering_cmd_2,
self.gear_cmd, 1, 1, 1, checksum
]
# Log Info
rospy.loginfo(str(steering_cmd_raw))
rospy.loginfo(str(data_list))
msg.data = data_list
self.can_pub.publish(msg)
def angleToFrame(self):
frame = Frame()
frame.id = self._id
frame.is_rtr = False
frame.is_extended = False
frame.is_error = False
frame.dlc = 4
data = bytearray(struct.pack('f', self._pwm))
frame.data = str(data)
return frame
def motor_wheel_error(self, can_data):
can_id = 0x140
CanFrame = Frame()
CanFrame.id = can_id
CanFrame.dlc = 8
CanFrame.is_rtr = 0
CanFrame.is_extended = 0
CanFrame.is_error = 0
CanFrame.data = "\x02" + "\x03\x00\x00\x03\xE8\x00\x64"
self.cmd_pub.publish(CanFrame)
DisableCanFrame = Frame()
DisableCanFrame.id = can_id
DisableCanFrame.dlc = 8
DisableCanFrame.is_rtr = 0
DisableCanFrame.is_extended = 0
DisableCanFrame.is_error = 0
DisableCanFrame.data = "\x02" + "\x01\x00\x00\x00\x00\x00\x00"
self.cmd_pub.publish(DisableCanFrame)
print("error_data")
error_type = []
data_len = can_data.__len__()
for i in range(data_len):
error_left = can_data[i] >> 4 & 0xF
if (error_left > 0):
error_cont = WheelError(i).name + "_left "
error_type.append(error_cont)
# error_type.append("left ")
error_right = can_data[i] & 0xF
if (error_right > 0):
error_cont = WheelError(i).name + "_right "
error_type.append(error_cont)
# error_type.append("right ")
print(error_type)
def speed_dir_To_Frame(self):
frame = Frame()
frame.id = self.ID
frame.is_rtr = False
frame.is_extended = False
frame.is_error = False
frame.dlc = 8
data = bytearray(struct.pack('f', self.speed_dir))
frame.data = str(data)
return frame
def send_auger(self):
frame = Frame()
frame.id = self.ID_auger
frame.is_rtr = False
frame.is_extended = False
frame.is_error = False
frame.dlc = 4
data = bytearray(struct.pack('f', self.speed_dir))
frame.data = str(data)
return frame
def send_flap(self):
frame = Frame()
frame.id = self.ID
frame.is_rtr = False
frame.is_extended = False
frame.is_error = False
frame.dlc = 8
data = bytearray(struct.pack('i', self.position))
frame.data = str(data)
return frame
def send_temp_sensor(self):
frame = Frame()
frame.id = self.ID_temp_sensor
frame.is_rtr = False
frame.is_extended = False
frame.is_error = False
frame.dlc = 4
data = bytearray(struct.pack('i', self.temp_sensor))
frame.data = str(data)
return frame
def angleToFrame(self):
frame = Frame()
frame.id = self.ID
frame.is_rtr = False
frame.is_extended = False
frame.is_error = False
frame.dlc = 8
data = bytearray(struct.pack('i', self.pan))
data.extend(bytearray(struct.pack('i', self.tilt)))
frame.data = str(data)
return frame
def VelCallback(self, data):
twist = Twist()
twist = data
vel = twist.linear.x
delta =twist.angular.z
if( vel > 1000):
vel = 1000
elif ( vel < - 1000):
vel = -1000
if(delta > 200):
delta = 200
elif (delta < -200):
delta = -200
Canvel = np.float16(vel)
Candelta = np.float16(delta)
print(delta)
CanFrame = Frame()
CanFrame.id = int('0x141', 16)
index = np.uint8(int('0x02', 16))
module = np.uint8(int('0x23', 16))
command = np.uint8(int('0x00', 16))
channel = np.uint8(int('0x00', 16))
VelHex = self.float_to_hex(Canvel)
VelHex1 = np.uint8(VelHex >> 8 & 0xFF)
VelHex2 = np.uint8(VelHex & 0xFF)
DeltaHex = self.float_to_hex(Candelta)
DeltaHex1 = np.uint8(DeltaHex >> 8 & 0xFF)
DeltaHex2 = np.uint8(DeltaHex & 0xFF)
# data1 = (Canvel&255)
# data2 = (Canvel>>8)
# data3 = (Candelta&255)
# data4 = (Candelta>>8)
#CanFrame.data = index + module + command + channel + data1 + data2 + data3 + data4
# CanFrame.data[0] = 1
CanFrame.data = index + module + command + channel + VelHex1 + VelHex2 + DeltaHex1 + DeltaHex2
print('index={}, module={}, command={}, channel={}, vel={}, delta={}'.format(hex(CanFrame.data[0]), hex(CanFrame.data[1]), hex(CanFrame.data[2]), hex(CanFrame.data[3]), hex(VelHex), hex(DeltaHex))
def enable_coils_cb(self, msg):
frame = Frame()
frame.header.stamp = rospy.Time.now()
frame.id = MD_CAN_ID
frame.is_rtr = False
frame.is_extended = True
frame.is_error = False
frame.dlc = 2
frame.data = ''.join(
[chr(x) for x in 1 << 7 | ENABLE_COILS_REGISTER,
int(msg.data)])
self.can_pub.publish(frame)
def convert_panda_to_msg(can_msg):
frame = Frame()
frame.id = can_msg[0]
frame.dlc = 8
frame.is_error = 0
frame.is_rtr = 0
frame.is_extended = 0
frame.data = can_msg[2]
return frame
def motorenable(self):
can_id = 0x140
EnableCanFrame = Frame()
EnableCanFrame.id = can_id
EnableCanFrame.dlc = 8
EnableCanFrame.is_rtr = 0
EnableCanFrame.is_extended = 0
EnableCanFrame.is_error = 0
EnableCanFrame.data = "\x02" + "\x01\x00\x00\x00\x00\x01\x01"
self.cmd_pub.publish(EnableCanFrame)
def VelCallback(self, data):
twist = Twist()
twist = data
vel = twist.linear.x
delta = twist.angular.z
Canvel = vel * 1000
Candelta = delta * 10
Canvel = np.int(Canvel + 1000)
Candelta = np.int(Candelta + 100)
if (Canvel > 2000):
Canvel = 2000
if (Canvel < 0):
Canvel = 0
if (Candelta > 200):
Candelta = 200
if (Candelta < 0):
Candelta = 0
can_id = 0x140
"""
data = bytearray(8)
data[0] = 0x02
data[1] = 0x03
data[2] = 0x00
data[3] = 0x00
data[4] = Canvel >> 8 & 0xFF
data[5] = Canvel & 0xFF
data[6] = Candelta >> 8 & 0xFF
data[7] = Candelta & 0xFF
"""
CanFrame = Frame()
CanFrame.id = can_id
CanFrame.dlc = 8
CanFrame.is_rtr = 0
CanFrame.is_extended = 0
CanFrame.is_error = 0
CanFrame.data = "\x02" + "\x03\x00\x00" + chr(Canvel >> 8) + chr(
Canvel & 255) + chr(Candelta >> 8) + chr(Candelta & 255)
# for i in range(8):
# CanFrame.data[i] = "11"
self.cmd_pub.publish(CanFrame)
def convert_panda_to_msg(self, can_msg):
frame = Frame()
frame.id = can_msg[0]
frame.dlc = 8
frame.is_error = 0
frame.is_rtr = 0
frame.is_extended = 0
# hacky conversion to uint8
frame.data = str(can_msg[2])
frame.header.frame_id = ""
frame.header.stamp = rospy.get_rostime()
return frame
def on_message_received(self, msg):
cmd = Twist()
can_frame = Frame()
can_frame.data = msg.data
can_frame.dlc = msg.dlc
can_frame.id = msg.arbitration_id
print(can_frame.data)
if can_frame.id == 0x5FF:
(mode, linear, angular,
jockeyspeed) = struct.unpack('<Bhhh', can_frame.data)
cmd.linear.x = float(linear) / 1000
cmd.angular.z = float(angular) / 1000
cmd.linear.z = float(jockeyspeed)
print("mode %d \n"
"linear %d \n"
"angular %d \n"
"jockey %d \n" % (mode, linear, angular, jockeyspeed))
else:
print(False)
self.canpub.publish(cmd)
def control_callback(control, can_device='can0'):
rospy.loginfo("A control command %s was received on control topic", control)
# not implemented yet, just send something to verify it is communicating with the can bus.
# Note: sent_messages is the ROS topic where socketcan_bridge expects messages,
# since it is not configurable, it is hardcoded here.
socketcan_pub = rospy.Publisher('sent_messages', Frame, queue_size=1)
command = control.command
# Do not do anything if the command is invalid.
if command not in CONTROL_CODES:
rospy.loginfo("Command %s not found in %s", command, CONTROL_CODES.keys())
return
# If it is a valid command, let's get the hex code.
code = CONTROL_CODES[command]
rospy.loginfo("Command %s (%s) is being sent to the car", command, code)
# Plan B.
if can_device:
from subprocess import call
call(['/usr/bin/cansend', can_device, '358#00%02x000000' % code])
return
frame = Frame()
frame.id = 0x358
frame.data = [0, code, 0, 0, 0, 0, 0, 0]
frame.dlc = 8
frame.is_rtr = False
frame.is_extended = False
frame.is_error = False
rospy.loginfo(frame)
socketcan_pub.publish(frame)
rospy.loginfo("A can frame was sent to 'sent_messages': %s", frame)
sonar.radiation_type = 1
sonar.header.frame_id = 'sonar_left'
sonar.header.stamp = current_time
sonar.min_range = .020
sonar.max_range = 3
sonar.range = distance_fl / 100
sonar_pub_fl.publish(sonar)
sonar.field_of_view = .349
sonar.radiation_type = 1
sonar.header.frame_id = 'sonar_right'
sonar.header.stamp = current_time
sonar.min_range = .020
sonar.max_range = 3
sonar.range = distance_fr / 100
sonar_pub_fr.publish(sonar)
CAN_frame.header.stamp = current_time
CAN_frame.id = 0x177
CAN_frame.header.frame_id = 'USS'
CAN_frame.data = [
0x00,
np.uint8(distance_fl),
np.uint8(distance_fr), 0x00, 0x00, 0x00, 0x00, 0x00
]
CAN_frame.is_rtr = False
CAN_frame.is_extended = False
CAN_frame.is_error = False
CAN_frame.dlc = 8
CAN_publisher.publish(CAN_frame)