-
Notifications
You must be signed in to change notification settings - Fork 0
/
agv_base_Food_v2.py
1680 lines (1413 loc) · 58 KB
/
agv_base_Food_v2.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
##### Note: This file "Neglect" the CRC16 checksum which sould be applied in the Modbus Potocol. #####
#!/usr/bin/env python2
import rospy
import sys
import time
import math
import tf
import tf2_ros
import Queue
import copy
import threading
#
import json
#
import serial
import crcmod # CRC16 generation and checking
import struct # For packing/unpacking the byte array
import socket
from sensor_msgs.msg import Imu #, JointState
from tf.transformations import quaternion_from_euler
#
from geometry_msgs.msg import Twist, TransformStamped
from std_msgs.msg import (
Float32,
Float64,
Bool,
Empty,
String
)
from nav_msgs.msg import Odometry
# from agv.msg import(
# io_out_feedback,
# power_state,
# alarm_feedback
# )
#
# from std_msgs.msg import Float32MultiArray
# Settings
is_broadcasting_tf = True
#TCP ethernet
tcp_host = '192.168.1.1' # Product Car
#tcp_host = '192.168.52.239' # Test Car
tcp_port = 502
#tcp_host = '127.0.0.1'
#tcp_port = 7000
tcp_socket = None
value_x = 0
value_y = 0
# Serial port
serial_device_name = "/dev/base_USB"
#serial_device_name = "/dev/ttyUSB0"
serial_baud_rate = 115200 # 57600 # 115200
# Buffer for serial communication with base
rec_buff = ""
# CRC16
crc16_key = 0x11021 # int
#crc16_key = 0x18005 # int
crc16 = crcmod.mkCrcFun(crc16_key, rev=False)
# Special chars
head_char = b'\x01' # 0x01
end_chars = b'\x0d\x0a' # \r\n
#
serialDev = None
is_serial_device_available = False
# Publishers
serialOnlinePub = rospy.Publisher('/base/online', Bool, queue_size = 2, latch=True) # Latched
# Note: While using Cartographer's odom, the odom topic below should not be published.
#odom_pub = rospy.Publisher('/odom', Odometry, queue_size = 1)
# lift_height_pub = rospy.Publisher('/base/status/lift_height', Float64, queue_size = 1, latch=True)
# base_IO_pub = rospy.Publisher('/base/status/IO', kenmec_base_io_out, queue_size = 1, latch=True)
pause_move_pub = rospy.Publisher('/base/lock', Bool, queue_size = 1, latch=True) # This topic is mainly send to base_serial
pause_move_status_pub = rospy.Publisher('/base/status/pause_move', String, queue_size = 1, latch=True) # This topic is mainly send to base_serial
# io_out_feedback_pub = rospy.Publisher('/base/status/io_out_feedback', io_out_feedback, queue_size = 1, latch=True)
# power_state_pub = rospy.Publisher('/base/status/power', power_state, queue_size = 1, latch=True)
# alarm_feedback_pub = rospy.Publisher('/base/status/alarm_feedback', alarm_feedback, queue_size = 1, latch=True)
# joint_state_pub = rospy.Publisher('/joint_states', JointState, queue_size = 5, latch=True)
# tf broadcaster
odom_broadcaster = tf2_ros.TransformBroadcaster()
# Messages
# Time stamps
last_send_time = None
last_receiving_time = None
# send-data buffer
reset_list = [0.0, 0.0, 0.0, 0.0]
cmd_vel_list = [1.0, 0.0, 0.0, 0.0] # vel_x, vel_y, omega_z
# Variables
#------------------------#
x = 0.0
y = 0.0
th = 0.0
left_rpm = 0.0
right_rpm = 0.0
#
is_lock = False # Initialized to false
#
fork_plate_height = 0.0
#------------------------#
# last_vel_x = 0.0
# last_vel_y = 0.0
# last_vel_th = 0.0
# last_odom_time = time.time()
# ODOM_DELTA_TIME = 0.2
# Constants
#------------------------#
deg2rad = 0.017453292519943295 # pi/180.0
rad2deg = 57.29577951308232 # 180.0/pi
#------------------------#
# Program start time
stamp_prog_start = time.time()
stamp_rec_previous = time.time()
# odom and tf
#------------------------------------------------------#
def proc_odometry(vel_x, vel_y, omega_z):
global current_time
global last_time
global x,y,th
# global last_vel_x, last_vel_y, last_vel_th
#
current_time = rospy.Time.now()
dt = (current_time - last_time).to_sec()
last_time = current_time
#
delta_x = (vel_x * math.cos(th) - vel_y * math.sin(th) ) * dt
delta_y = (vel_x * math.sin(th) + vel_y * math.cos(th) ) * dt
delta_th = omega_z * dt
x += delta_x
y += delta_y
th += delta_th
#
pubodom(vel_x, vel_y, omega_z)
# pubodom(last_vel_x, last_vel_y, last_vel_th)
if is_broadcasting_tf:
broadcastodom()
# print "theta =", (th*57.2957795), "deg"
def broadcastodom():
global x,y,th
global odom_broadcaster
# odom_broadcaster = tf2_ros.TransformBroadcaster()
odom_quat = tf.transformations.quaternion_from_euler(0,0,th)
odom_trans = TransformStamped()
odom_trans.header.stamp = current_time
odom_trans.header.frame_id = "odom"
odom_trans.child_frame_id = "base_footprint" #"rugby_base"
odom_trans.transform.translation.x = x
odom_trans.transform.translation.y = y
odom_trans.transform.translation.z = 0.0
odom_trans.transform.rotation.x = odom_quat[0]
odom_trans.transform.rotation.y = odom_quat[1]
odom_trans.transform.rotation.z = odom_quat[2]
odom_trans.transform.rotation.w = odom_quat[3]
# print("[base] broadcast the odom-->base_footprint tf")
odom_broadcaster.sendTransform(odom_trans)
def pubodom(vel_x, vel_y, omega_z):
global x,y,th
global odom_pub
# odom_pub = rospy.Publisher('/odom', Odometry, queue_size = 50)
odom_quat = tf.transformations.quaternion_from_euler(0,0,th)
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "odom"
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation.x = odom_quat[0]
odom.pose.pose.orientation.y = odom_quat[1]
odom.pose.pose.orientation.z = odom_quat[2]
odom.pose.pose.orientation.w = odom_quat[3]
odom.pose.covariance[0] = 1
odom.pose.covariance[7] = 1
odom.pose.covariance[14] = 1
odom.pose.covariance[21] = 1
odom.pose.covariance[28] = 1
odom.pose.covariance[35] = 1
odom.child_frame_id = "base_footprint" #"rugby_base"
odom.twist.twist.linear.x = vel_x
odom.twist.twist.linear.y = vel_y
odom.twist.twist.angular.z = omega_z
odom_pub.publish(odom)
#------------------------------------------------------#
# Definition of pubMsgGroup
# Using string as key
#-----------------------------------#
"""
motion
query
turnSig
alarm
pauseButton
engagement
errorState
IOChange
rangeSense
IOIn
IOOut
power
radar_layer
"""
pubMsgGroup_func_dict = dict()
#-----------------------------------#
# joint_state_pub_rate = 10.0 # Hz
# def joint_state_sending_worker():
# """
# This is the worker for the thread of periodically sending the joint state of the Lift
# """
# global fork_plate_height
# rate = rospy.Rate(joint_state_pub_rate)
# print("[base] joint_state_sending_worker thread started.")
# while not rospy.is_shutdown():
# send_single_joint_state("fork_plat_joint", fork_plate_height)
# rate.sleep()
# print("[base] joint_state_sending_worker thread ended.")
# def send_single_joint_state(name, position, velocity=0.0, effort=0.0, stamp=None):
# """
# """
# global joint_state_pub
# # Broadcast joint state of the lift
# #-------------------------------------------------#
# _js = JointState()
# _js.header.stamp = rospy.get_rostime() if stamp is None else stamp
# _js.name.append(name)
# _js.position.append(position)
# _js.velocity.append(velocity)
# _js.effort.append(effort)
# joint_state_pub.publish(_js)
# #-------------------------------------------------#
# Managing the pause move signal state (ultimately it will be connected /base/lock)
#-------------------------------------#
pause_move_status_dict = dict()
required_to_pause = False
def reset_pause_move():
"""
Note: This function is not thread safe with update_pause_move(), only for testing.
"""
global pause_move_status_dict
global required_to_pause
for _key in pause_move_status_dict:
pause_move_status_dict[_key] = False
required_to_pause = False
pause_move_pub.publish(required_to_pause)
rospy.loginfo("[base] reset pause_move, required_to_pause = %s" % str(required_to_pause) )
# def publish_pause_move(value_list):
# """
# value_list: any one of the element is True will result in pause moving.
# """
# global required_to_pause
# # Scan
# required_to_pause_new = False
# triggered_idx_list = []
# for _i, _v in enumerate(value_list):
# required_to_pause_new |= _v
# if _v:
# triggered_idx_list.append(_i)
# # Publish
# if required_to_pause != required_to_pause_new:
# pause_move_pub.publish(required_to_pause_new)
# required_to_pause = required_to_pause_new
# print("[base] required_to_pause = %s, triggered_idx_list = %s" % (str(required_to_pause), str(triggered_idx_list) ) )
# return required_to_pause
def update_pause_move(new_status_dict):
"""
new_status_dict:
- key: the control item
- value: the value of the related control item, True: pause, False: safe
Any one of the element being True results in base locking.
"""
global pause_move_status_dict
global required_to_pause
# Update
pause_move_status_dict.update(new_status_dict)
triggered_key_list = []
required_to_pause_new = False
should_send_log = False
for _key, _value in pause_move_status_dict.items():
if _value: # Only enter this clause if the value is True
required_to_pause_new = True
# Immediate publish the message (early stopping, but keep collecting the keys with value True)
if required_to_pause != required_to_pause_new:
pause_move_pub.publish(required_to_pause_new)
required_to_pause = required_to_pause_new # Note: This prevent the entering of this clause at next key with value True.
should_send_log = True
# Collecting the keys with value True
triggered_key_list.append(_key)
# Publish when the value from True --> False (no early stopping --> all False)
if required_to_pause != required_to_pause_new:
pause_move_pub.publish(required_to_pause_new)
required_to_pause = required_to_pause_new
should_send_log = True
# log
if should_send_log:
rospy.loginfo("[base] required_to_pause = %s, triggered_key_list = %s" % (str(required_to_pause), str(triggered_key_list) ) )
return required_to_pause
#-------------------------------------#
# Receiving-data processing { NEED TO MODIFIED!!!!!!!!!!!!!!!!!!!!!!!!!!! }
#-------------------------------------------------#
def pubMsgGroup_command(ticket, rec_data_list):
print("send command")
def pubMsgGroup_motion(ticket, rec_data_list):
#
# vel_x = rec_data_list[3]
# vel_y = rec_data_list[4]
# omega_z = rec_data_list[5]
#print("<%s>(vel_x, vel_y, omega_z) = %s" % (ticket["pubMsgGroup"], str((vel_x, vel_y, omega_z))) )
# proc_odometry(vel_x, vel_y, omega_z)
pass
def pubMsgGroup_query(ticket, rec_data_list):
#
# vel_x = rec_data_list[3]
# vel_y = rec_data_list[4]
# omega_z = rec_data_list[5]
#print("<%s>(vel_x, vel_y, omega_z) = %s" % (ticket["pubMsgGroup"], str((vel_x, vel_y, omega_z))) )
# proc_odometry(vel_x, vel_y, omega_z)
pass
# def pubMsgGroup_lift(ticket, rec_data_list):
# """
# output
# - height_now (unit: m): Note: the numeric in data frame from base in the unit of mm
# """
# global fork_plate_height
# height_now = rec_data_list[4] * 0.001
# print("<%s>height_now = %f" % (ticket["pubMsgGroup"], height_now))
# lift_height_pub.publish(height_now)
# # Broadcast joint state of the lift
# #-------------------------------------------------#
# fork_plate_height = copy.copy(height_now)
# send_single_joint_state("fork_plat_joint", fork_plate_height)
# #-------------------------------------------------#
def pubMsgGroup_alarm():
print("send alarm")
pass
def pubMsgGroup_io_out(ticket, rec_data_list):
# status = rec_data_list[4]
# bit_str = get_bit_str_from_uint16[status]
# auto_charge = get_bit_from_bit_str(bit_str, 0)
# left_light = get_bit_from_bit_str(bit_str, 1)
# right_light = get_bit_from_bit_str(bit_str, 2)
# red_light = get_bit_from_bit_str(bit_str, 3)
# green_light = get_bit_from_bit_str(bit_str, 4)
# yellow_light = get_bit_from_bit_str(bit_str, 5)
# buzzer = get_bit_from_bit_str(bit_str, 6)
# speaker = get_bit_from_bit_str(bit_str, 7)
# msg = io_out_feedback()
# msg.auto_charge = auto_charge
# msg.left_light = left_light
# msg.right_light = right_light
# msg.red_light = red_light
# msg.green_light = green_light
# msg.yellow_light = yellow_light
# msg.buzzer = buzzer
# msg.speakerA = speakerA # On Duty Speaker
# msg.speakerB = speakerB # Deceleration Speaker
# msg.speakerC = speakerC # Charging Speaker
# io_out_feedback_pub.publish(msg)
# print("auto_charge = " + auto_charge)
# print("left_light = " + left_light)
# print("right_light = " + right_light)
# print("red_light = " + red_light)
# print("green_light = " + green_light)
# print("yellow_light = " + yellow_light)
# print("buzzer = " + buzzer)
# print("on duty speaker = " + speakerA)
# print("deceleration speaker = " + speakerB)
# print("charge speaker = " + speakerC)
pass
def pubMsgGroup_turn_sign():
print("send turn signal")
pass
def pubMsgGroup_power(ticket, rec_data_list):
#v_bat = rec_data_list[3]
#i_bat = rec_data_list[4]
print("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% v " + str(rec_data_list[3]))
print("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% i " + str(rec_data_list[4]))
soc = rec_data_list[5]
print("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% soc " + str(rec_data_list[5]))
soc = rec_data_list[5]
#print("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%<%s>(v_bat, i_vat, SoC) = %s" % (ticket["pubMsgGroup"], str((v_bat, i_bat, soc))))
msg = power_state()
msg.v_bat = 0
msg.i_bat = 0
msg.soc = soc
power_state_pub.publish(msg)
def pubMsgGroup_radar_layer(ticket, rec_data_list):
print("send turn radar_layer")
pass
def pubMsgGroup_io_in(ticket, rec_data_list):
print("send io_in")
pass
#-------------------------------------------------#
# Register the functions for pubMsgGroup
#-------------------------------------------------#
pubMsgGroup_func_dict["motion"] = pubMsgGroup_motion #pubMsgGroup_command
pubMsgGroup_func_dict["query"] = pubMsgGroup_query
# pubMsgGroup_func_dict["lift"] = pubMsgGroup_lift
pubMsgGroup_func_dict["alarm"] = pubMsgGroup_alarm
pubMsgGroup_func_dict["IOOut"] = pubMsgGroup_io_out
pubMsgGroup_func_dict["turnSig"] = pubMsgGroup_turn_sign
pubMsgGroup_func_dict["power"] = pubMsgGroup_power
pubMsgGroup_func_dict["radar_layer"] = pubMsgGroup_radar_layer
pubMsgGroup_func_dict["IOIn"] = pubMsgGroup_io_in
#-------------------------------------------------#
def get_bit_str_from_uint16(num_uint16_in):
"""
Example
num_uint16_in = 0x25 (dec: 37)
bit_str_out = "0000000000100101"
** Note: MSB is at the left. Index of the MSB is 0 **
"""
return ( '{:016b}'.format(num_uint16_in) )
def get_bit_from_bit_str(bit_str, bit_idx=0):
"""
"bit" string: a len-16 string with each char being '0' (string) or '1' (string)
e.g. "0001", bit #0 is 1 (True), where bit #3 is 0 (False)
return True/False (1/0)
"""
return (bit_str[-(1+bit_idx)] == '1') # Index from the back of the bit_str
def get_bit_list_from_bit_str(bit_str):
"""
"bit" string: a len-16 string with each char being '0' (string) or '1' (string)
e.g. "0001", bit #0 is 1 (True), where bit #3 is 0 (False)
return [True/False (1/0)] (a list)
"""
bit_list = list()
for bit_idx in range(len(bit_str)):
bit_list.append( (bit_str[-(1+bit_idx)] == '1') ) # Index from the back of the bit_str
return bit_list
#----------------------------------------#
# def process_event_0x33(data):
# """
# """
# # Convert to "bit" string - a len-16 string with each char being '0' (string) or '1' (string)
# # bit_str_data_1_2 = '{:016b}'.format(data)
# bit_str_data_1_2 = get_bit_str_from_uint16( data )
# bit_str_data_1 = bit_str_data_1_2[:8]
# bit_str_data_2 = bit_str_data_1_2[8:]
# print("bit_str_data_1 = [%s]" % bit_str_data_1)
# print("bit_str_data_2 = [%s]" % bit_str_data_2)
# #
# bit_list_data_1 = get_bit_list_from_bit_str(bit_str_data_1)
# bit_list_data_2 = get_bit_list_from_bit_str(bit_str_data_2)
# # Assign to each element
# # touch_L, touch_R, approx_L, approx_R, bumper, reset_button, TIM320_L_out1, TIM320_L_out2 = bit_list_data_1
# # TIM320_L_out3, TIM320_L_out4, TIM320_R_out1, TIM320_R_out2, TIM320_R_out3, TIM320_R_out4, range_check_L, range_check_R = bit_list_data_2
# touch_L, touch_R, approx_L, approx_R, bumper, reset_button, range_check_L, range_check_R = bit_list_data_1
# TIM320_L_out1, TIM320_L_out2, TIM320_L_out3, TIM320_L_out4, TIM320_R_out1, TIM320_R_out2, TIM320_R_out3, TIM320_R_out4 = bit_list_data_2
# _io_msg = kenmec_base_io_out()
# _io_msg.touch_L = touch_L
# _io_msg.touch_R = touch_R
# _io_msg.approx_L = approx_L
# _io_msg.approx_R = approx_R
# _io_msg.bumper = bumper
# _io_msg.reset_button = reset_button
# _io_msg.TIM320_L_out1 = TIM320_L_out1
# _io_msg.TIM320_L_out2 = TIM320_L_out2
# _io_msg.TIM320_L_out3 = TIM320_L_out3
# _io_msg.TIM320_L_out4 = TIM320_L_out4
# _io_msg.TIM320_R_out1 = TIM320_R_out1
# _io_msg.TIM320_R_out2 = TIM320_R_out2
# _io_msg.TIM320_R_out3 = TIM320_R_out3
# _io_msg.TIM320_R_out4 = TIM320_R_out4
# _io_msg.range_check_L = range_check_L
# _io_msg.range_check_R = range_check_R
# print(str(_io_msg))
# base_IO_pub.publish(_io_msg)
# # Test, base lock directly issued by bumper
# # pause_move_pub.publish( bumper)
# # publish_pause_move( (bumper, TIM320_L_out1, TIM320_R_out1) )
# new_status_dict = dict()
# new_status_dict["bumper"] = bumper
# new_status_dict["TIM320_L_out1"] = TIM320_L_out1
# new_status_dict["TIM320_R_out1"] = TIM320_R_out1
# update_pause_move(new_status_dict)
# #
#----------------------------------------#
def process_event(valid_data_frame):
"""
This is the function for parsing the event and pubilish it.
"""
print("Event: valid_data_frame = 0x{}".format( valid_data_frame.encode('hex')))
# rec_data_list = struct.unpack('>BBHH', valid_data_frame[:-2]) # Big-endian, remove the CRC16 part
print("Event: rec_data_list = {}".format(rec_data_list))
# TODO: Publish the event according to the CMD address
if rec_data_list[2] == int("33", 16):
process_event_0x33(rec_data_list[3])
else:
pass
#
return rec_data_list
#----------------------------------------#
# New data receiving process
#-----------------------------------------------------#
# data_frame_format for motion feedback '>BBHfff'
def _serialRead(data_frame_length=6, data_frame_format='>BBHH', read_once=False):
"""
Wait for the specified response frame and event from serial port.
Inputs:
data_frame_length: The length of the specified data frame (Note: CRC16 bytes are not included)
data_frame_format: The format of the data frame (Note: the CRC16 bytes are not included)
Note: Default arguments are set for the event frame.
"""
global stamp_prog_start, stamp_rec_previous
global rec_buff, is_serial_device_available
global crc16, head_char, end_chars
# Return if the serial is not available
if not is_serial_device_available:
return
# Parameter for receiveing data
MAX_BUF_SIZE = 1000 # 2500 # For a package being 50 bytes, 50 Hz transmition rate, this is a 1 sec. buffer
RECEIVED_TIMEOUT = 0.15 # 0.2 # 5.0 # sec.
# Read serial
is_frame_completed = False
count_no_data = 0
count_loop = 0
is_first_time = True
#
# data_frame_length_CRC = data_frame_length + 2 # The CRC16 contains 2 bytes
# data_frame_length_target = data_frame_length_CRC
# if not read_once:
# print("data_frame_length = %d" % data_frame_length)
# print("data_frame_length_CRC = %d" % data_frame_length_CRC)
# print("data_frame_length_target = %d" % data_frame_length_target)
is_event = False
stamp_start = time.time()
#
rec_data_list = None
delta_time = 0.0
while not is_frame_completed:
count_loop += 1
# Timeout check
delta_prog_time = time.time() - stamp_prog_start
delta_time = time.time() - stamp_start
if delta_time > RECEIVED_TIMEOUT:
print("[WARN]Receive-timeout: %f > %f" % (delta_time, RECEIVED_TIMEOUT))
break
# Sleep for a while (1ms) when looping
if is_first_time:
is_first_time = False
else:
if read_once:
break
time.sleep(0.001)
# time.sleep(0.1)
# Roger: If the request will not be response, set frame completed
if data_frame_length == 0 and data_frame_format == '>XXXX':
is_frame_completed = True
break
# Read from buffer
try:
rec_delta = serialDev.read(serialDev.inWaiting())
except:
#is_serial_device_available = False
break
# if not read_once:
# print("delta_time = %f" % delta_time)
# print(rec_buff)
# print("rec_delta = 0x{}".format( rec_delta.encode('hex')))
# print("rec_buff = 0x{}".format( rec_buff.encode('hex')))
# if read_once:
# print("delta_time = %f" % delta_time)
# print(rec_buff)
# print("rec_delta = 0x{}".format( rec_delta.encode('hex')))
# print("rec_buff = 0x{}".format( rec_buff.encode('hex')))
# See if we got data
# if no data received this time, bypass the parsing this time if it's not read_once for efficiency
#----------------------#
if len(rec_delta) == 0 and (not read_once):
count_no_data += 1
# print("no data this time")
continue
else:
count_no_data = 0
if not read_once:
# print("rec_buff (before appending) = 0x{}".format( rec_buff.encode('hex')))
print("--Received data from serial port, time elapsed = %f sec., count_loop = %d" % (delta_time, count_loop))
print("--rec_delta = 0x{}".format( rec_delta.encode('hex')))
#----------------------#
# Add to the buffer
rec_buff += rec_delta
size_rec_buf = len(rec_buff)
if not read_once:
print("delta_prog_time = %f" % delta_prog_time)
print(rec_buff)
print("rec_delta = 0x{}".format( rec_delta.encode('hex')))
print("rec_buff = 0x{}".format( rec_buff.encode('hex')))
idx_head = rec_buff.find(head_char)
# if not read_once:
# print("rec_buff = 0x{}".format( rec_buff.encode('hex')))
# print("idx_head = %d" % idx_head)
if idx_head == -1:
# "head" not found,
# continue
# Clean data when the buffer is too long
if size_rec_buf >= MAX_BUF_SIZE:
rec_buff = ""
continue
# Check the length for least valid data (empty)
if (size_rec_buf-idx_head) < 4:
# Nomatter which function we used, the length of data frame is at least 4 bytes (id|func|...|CRC-H|CRC-L)
# It's not posssible to find a valid data frame within this short data,
# keep waiting
continue
# Found the plausible head. Further check
# Check if it's event (Note: the indexing is valid because the length of the data is guarenteed to be more than 4 bytes)
if rec_buff[idx_head+1] == b'\x08': # Event
data_frame_length_target = 8 # Temporary modify the task to receive the event
is_event = True
print("<--(jump) Waiting for event")
else:
data_frame_length_target = data_frame_length_CRC # Return to the original target
is_event = False
# Check the length for the require data frame
if (size_rec_buf - idx_head) < data_frame_length_target:
# Frame not completed, keep waiting
continue
# We got the full data frame, check if it's valid (CRC)
idx_end = idx_head + data_frame_length_target # Note: idx_end points to teh char that is not in the frame
print("--rec_buff (found a plaussible data segment with sufficient length and correct head)\n\t\t= 0x{}".format( rec_buff.encode('hex')))
print("--data_frame_length_target = %d" % data_frame_length_target)
print("--(idx_head, idx_end) = (%d, %d)" % (idx_head, idx_end))
# 2. CRC16 checksum
# Generate the crc16 checksum, should be the same as the one the message carried
# buff_crc_int = crc16( rec_buff[idx_head:(idx_end-2)] )
# print("buff_crc = {}".format( hex(buff_crc_int) ) )
# Checksum, the inverse calculation includes the crc16 code
buff_checksum_int = crc16( rec_buff[idx_head:idx_end] )
print("--buff_checksum_int = %d" % buff_checksum_int)
# Should be exactly zero
if buff_checksum_int != 0:
print("[WARN]Checksum test failed!!")
# Remove the current head (It's not sufficient to show that the rest char are invalid data)
rec_buff = rec_buff[(idx_head+1):]
continue
# Parse data
valid_data_frame = rec_buff[idx_head:idx_end]
# Trim frame
rec_buff = rec_buff[idx_end:]
#
state_rec_current_time = time.time()
stamp_rec_delta = state_rec_current_time - stamp_rec_previous
stamp_rec_previous = state_rec_current_time
print("--Found valid data frame with CRC check passed, time elapsed = %f sec., count_loop = %d" % (delta_time, count_loop))
print("--valid_data_frame = 0x{}".format( valid_data_frame.encode('hex')))
print("--Delta time from previous receiving = %f" % stamp_rec_delta)
#
# Different processing routing for event and required response
if is_event:
# Parse and send the event
process_event(valid_data_frame)
print("-->(return) Finish the evet processing, keep waiting for the required response.")
# Continue for the main receiving task
continue
else:
# Parse the data in the given format
try:
rec_data_list = struct.unpack(data_frame_format, valid_data_frame[:-2]) # Remove the CRC16 part
print(">>>(Got) rec_data_list = {}".format(rec_data_list))
except:
print("[WARN]Received valid non-event frame does not match the given data frame, break.")
break
# # Note: the following code is only for demo.
# # Represented by dict()
# rec_data_dict = dict()
# rec_data_dict['id'] = rec_data_list[0]
# rec_data_dict['rw'] = rec_data_list[1]
# rec_data_dict['msg_type'] = rec_data_list[2]
# rec_data_dict['x'] = rec_data_list[3]
# rec_data_dict['y'] = rec_data_list[4]
# rec_data_dict['yaw'] = rec_data_list[5]
# print("rec_data_dict = %s" % json.dumps(rec_data_dict, indent=4))
# End of the receiving task, leave
is_frame_completed = True
break # break the while
# end while
if is_frame_completed:
print(">>>(Got) Data received, time elapsed = %f sec., count_loop = %d" % (delta_time, count_loop))
else: # May be tiemout, data not received
if not read_once:
print("Receiving task was not completed, timeout/invalid-frame.")
#
return rec_data_list
#-----------------------------------------------------#
# New data sending process
#-----------------------------------------------------#
def _serialSend(send_data_list, data_frame_format='>BBHfff'):
"""
Input:
- send_data_list: list of data which are ready to be serialized as string (Note: CRC16 is not included.)
- data_frame_format: The format for the data frame. (Note: CRC16 is not included, default: motion cmd.)
"""
global last_send_time
global crc16, end_chars
if len(send_data_list) != (len(data_frame_format)-1):
print("send_data_list (cell:%d) does not match with data_frame_format (cell:%d), no sending" % (len(send_data_list), (len(data_frame_format)-1)) )
return
# # Generate data_string. This should be "bytes" (str)
# data_string = struct.pack(data_frame_format, 1, 3, int(cmd_group), \
# cmd_list[1], \
# cmd_list[2], \
# cmd_list[3])
data_string = struct.pack(data_frame_format, *send_data_list)
# Test
# send_buff = "01030001bdcccccdbe4ccccdbe99999a".decode("hex")
# CRC16 checksum
# Generate the crc16 checksum, should be the same as the one the message carried
buff_crc_int = crc16( data_string )
buff_crc_str = struct.pack('>H', buff_crc_int)
data_string += buff_crc_str
# end_chars
# data_string += end_chars
# Try to send down
try:
print("send start......")
tcp_socket.send(data_string)
print("send end......")
indata = tcp_socket.recv(1024)
#serialDev.write(data_string)
# serialDev.write("Hello")
except:
is_serial_device_available = False
print("-"*20)
print("<<<(send)buff_crc_int = %d" % buff_crc_int)
print("<<<(send)buff_crc = 0x{}".format( buff_crc_str.encode('hex') ) )
print("<<<(send)data_string = 0x{}".format(data_string.encode('hex')))
print("-"*20)
"""
tcp_indata = tcp_socket.recv(256)
if len(tcp_indata) == 0: # connection closed
tcp_socket.close()
print("tcp socket recv error.........")
is_serial_device_available = False
print("recv: 0x{}".format(tcp_indata.encode('hex')))
"""
#-----------------------------------------------------#
# Communication engine (request-response type)
#------------------------------------------------#
base_req_Q = Queue.Queue() # Unlimited length
ticket_dropping_threshold = 0.2 # sec.
def book_request(pubMsgGroup,
send_data_list,
send_data_frame_format='>BBHfff',
rec_data_frame_length=6,
rec_data_frame_format='>BBHfff'):
"""
This is the function for booking a request.
"""
global base_req_Q
ticket = dict()
ticket["pubMsgGroup"] = pubMsgGroup
ticket["send_data_list"] = send_data_list
ticket["send_data_frame_format"] = send_data_frame_format
ticket["rec_data_frame_length"] = rec_data_frame_length
ticket["rec_data_frame_format"] = rec_data_frame_format
ticket["stamp"] = time.time()
base_req_Q.put(ticket)
print("@(booking) A ticket booked for <%s>" % ticket["pubMsgGroup"] )
def communication_engine():
"""
A routine process for handling request, if any
"""
global base_req_Q
global ticket_dropping_threshold
# global last_vel_x, last_vel_y, last_vel_th, last_odom_time
while ( (not base_req_Q.empty()) and (not rospy.is_shutdown()) ):
# while base_req_Q.qsize() > 3.0:
# base_req_Q.get()
# Non-empty
ticket = base_req_Q.get()
# Flushing out old commands
_current_stamp = time.time()
if ticket_dropping_threshold is not None:
_dropout_stamp = _current_stamp - ticket_dropping_threshold
while ticket["stamp"] < _dropout_stamp and ticket["pubMsgGroup"] == "motion":
print("!! !!(handling) The ticket of <%s> got from the queue is too old, <%f> sec. ago. Drop out.." % (ticket["pubMsgGroup"], (_current_stamp - ticket["stamp"]) ) )
try:
ticket = base_req_Q.get(block=False)
print("D: %f ms" % (1000.0*(time.time() - _current_stamp)) )
except:
ticket = None
break
if ticket is None:
break
#
print("!!(handling) A ticket of <%s> got from the queue. Number of remained ticket(s) in the queue = %d" % (ticket["pubMsgGroup"], base_req_Q.qsize()) )
rec_data_list = _communication_core(
ticket["send_data_list"],
ticket["send_data_frame_format"],
ticket["rec_data_frame_length"],
ticket["rec_data_frame_format"]
)
# Processing the data
print(">>>(got) rec_data_list from serialRead = %s" % str(rec_data_list))
if rec_data_list is not None:
pubMsgGroup_func_dict[ ticket["pubMsgGroup"] ](ticket, rec_data_list)
# try:
# pubMsgGroup_func_dict[ ticket["pubMsgGroup"] ](ticket, rec_data_list)
# except:
# print("No function registered for [%s]" % ticket["pubMsgGroup"])
# Sleep for a while in case that the base cannot process the request right after the previous request
time.sleep(0.001)
print("-"*20)
print("\n")
# # check odom
# odom_delta_time = time.time() - last_odom_time
# if odom_delta_time > ODOM_DELTA_TIME:
# last_vel_x = 0.0
# last_vel_y = 0.0
# last_vel_th = 0.0
# pubodom(last_vel_x, last_vel_y, last_vel_th)
# last_odom_time = time.time()
# Still, do a polling once
# Clean-up read (parse and send event if there is any)
_serialRead(read_once=True)
def _communication_core(send_data_list,
send_data_frame_format='>BBHfff',
rec_data_frame_length=6,
rec_data_frame_format='>BBHfff'):
"""
This is the engine to perform the request-response type of protocal
as well as the event trigger type protocal.
The engine also include a clean-up action for only receive the event and clean otherwise.
"""
# Clean-up read (parse and send event if there is any)
_serialRead(read_once=True)
# send
_serialSend(send_data_list, send_data_frame_format)
# read & parse (While it's waiting, parse and send event if there is any)
rec_data_list = _serialRead(rec_data_frame_length, rec_data_frame_format, read_once=False)
# rec_data_list = None
# return parsed data list in the given data frame
return rec_data_list
#------------------------------------------------#
# Multiple type of serial-request functions
#---------------------------------------------#
def SerialReq_motion( vel_x, vel_y, omega_z ):
"""
Request to set the motion command.
input:
"""
print "vel_x = ", vel_x # m/s
print "vel_y = ", vel_y
print "omega_z = ", omega_z # rad/s
# Calibrate the porportion of the wheel motor ( 0 for 0 m/min, 8000 for 50 m/min )
value_y = vel_x / 50 * 60 * 8000
# yaw_rate = (Rwheelspeed - Lwheelspeed) / d where d is the track width between two differential wheels
# for Food AMR, the maximun value of value_x = 4000 equals to the difference speed 22.4 m/min between two wheels.
if value_y == 0:
value_x = omega_z / 22.4 * 60 * 0.55 * 4000 * 2 # +/- correcting the control direction
else:
value_x = omega_z / 22.4 * 60 * 0.55 * 4000 # +/- correcting the control direction
if value_y >= 8000:
value_y = 8000
elif value_y <= -8000:
value_y = -8000
if value_x >= 4000:
value_x = 4000
elif value_x <= -4000:
value_x = -4000
#send_data_list = (0x01, 0x06, 0x0262, 1000)