From b6c3a952f385156c66a033627a6f83bf2096d1c4 Mon Sep 17 00:00:00 2001 From: Federica Di Lauro Date: Tue, 11 Feb 2020 16:49:50 +0100 Subject: [PATCH] receiving velocities command using protobuf --- otto_controller/Core/Src/main.cpp | 29 +- .../scripts/cmd_vel_transmitter.py | 9 +- .../scripts/cmd_vel_transmitter_backup.py | 42 +++ .../scripts/otto_communication_pb2.py | 304 ++++++++++++++++++ .../scripts/otto_communication_pb2.pyc | Bin 0 -> 7211 bytes .../joypad_bridge/scripts/proto_transmit.py | 36 +++ .../joypad_bridge/scripts/velocities_pb2.py | 76 ----- .../joypad_bridge/scripts/velocities_pb2.pyc | Bin 2459 -> 0 bytes .../py_serial_examples/config_command_pb2.py | 153 --------- utils/py_serial_examples/proto_transmit.py | 2 +- .../py_serial_examples/status_message_pb2.py | 135 -------- .../velocity_command_pb2.py | 76 ----- 12 files changed, 407 insertions(+), 455 deletions(-) create mode 100755 utils/catkin_ws/src/joypad_bridge/scripts/cmd_vel_transmitter_backup.py create mode 100644 utils/catkin_ws/src/joypad_bridge/scripts/otto_communication_pb2.py create mode 100644 utils/catkin_ws/src/joypad_bridge/scripts/otto_communication_pb2.pyc create mode 100644 utils/catkin_ws/src/joypad_bridge/scripts/proto_transmit.py delete mode 100644 utils/catkin_ws/src/joypad_bridge/scripts/velocities_pb2.py delete mode 100644 utils/catkin_ws/src/joypad_bridge/scripts/velocities_pb2.pyc delete mode 100644 utils/py_serial_examples/config_command_pb2.py delete mode 100644 utils/py_serial_examples/status_message_pb2.py delete mode 100644 utils/py_serial_examples/velocity_command_pb2.py diff --git a/otto_controller/Core/Src/main.cpp b/otto_controller/Core/Src/main.cpp index e47d2ad..d19ac9f 100644 --- a/otto_controller/Core/Src/main.cpp +++ b/otto_controller/Core/Src/main.cpp @@ -104,7 +104,7 @@ uint8_t *rx_buffer; velocity_msg vel_msg; wheel_msg wheels_msg; -uint8_t mode = 0; //setup mode +int mode = 0; //setup mode uint8_t proto_buffer_rx[50]; pb_istream_t in_pb_stream; @@ -315,30 +315,35 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) { + mode++; + float linear_velocity; float angular_velocity; - pb_istream_t stream = pb_istream_from_buffer(proto_buffer_rx, velocity_cmd_length); + pb_istream_t stream = pb_istream_from_buffer(proto_buffer_rx, + velocity_cmd_length); /* Now we are ready to decode the message. */ bool status = pb_decode(&stream, VelocityCommand_fields, &vel_cmd); - linear_velocity = vel_cmd.linear_velocity; - angular_velocity = vel_cmd.angular_velocity; + if (status) { + linear_velocity = vel_cmd.linear_velocity; + angular_velocity = vel_cmd.angular_velocity; - odom.UpdateValues(linear_velocity, angular_velocity); + odom.UpdateValues(linear_velocity, angular_velocity); - float left_setpoint = odom.GetLeftVelocity(); - float right_setpoint = odom.GetRightVelocity(); + float left_setpoint = odom.GetLeftVelocity(); + float right_setpoint = odom.GetRightVelocity(); - left_pid.set(left_setpoint); - right_pid.set(right_setpoint); + left_pid.set(left_setpoint); + right_pid.set(right_setpoint); - float cross_setpoint = left_setpoint - right_setpoint; - cross_pid.set(cross_setpoint); + float cross_setpoint = left_setpoint - right_setpoint; + cross_pid.set(cross_setpoint); + } HAL_UART_Receive_DMA(&huart6, (uint8_t*) &proto_buffer_rx, - velocity_cmd_length); + velocity_cmd_length); } void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { diff --git a/utils/catkin_ws/src/joypad_bridge/scripts/cmd_vel_transmitter.py b/utils/catkin_ws/src/joypad_bridge/scripts/cmd_vel_transmitter.py index fe9a4a2..36bf463 100755 --- a/utils/catkin_ws/src/joypad_bridge/scripts/cmd_vel_transmitter.py +++ b/utils/catkin_ws/src/joypad_bridge/scripts/cmd_vel_transmitter.py @@ -1,6 +1,7 @@ #!/usr/bin/env python import rospy, serial, struct, time +import otto_communication_pb2 from geometry_msgs.msg import Twist from serial import SerialException @@ -14,10 +15,14 @@ ser = serial.Serial( def callback(data): linear = -data.linear.x angular = -data.angular.z #da fixare? + my_velocity = otto_communication_pb2.VelocityCommand() + my_velocity.linear_velocity = linear; + my_velocity.angular_velocity = angular; rospy.loginfo('I heard %f %f', linear, angular) - msg_output = struct.pack('Z+Q2-p&^Wnfn@Ds~kSEKhcq2(SX|V~mhpqvtBY zuH&-mF0dZ~`!TSeKnGHlWy{*GUuonuvZw zM0&M0rbBd}h<-~%dc6}wRG^4%4o8GhziTWHh~jrdp=;$PQG7uZyYEj?p8@s|>Huty zp8L42b)#DoMar<=X9om0q~~2=U-oJuTe(K-tIME7u9m*941Q0rucluHkBI1K`ejfi zqT`V=_~3>8n27!`QVbuQ$R?r_BGS@xdpbMll!%@XkruDQbcmi3(K8~_^7ni?L@$V_ zLPXC=XLqMV^hYB46A|qa(aY%&eN99T5xpd$Z>B@!5>b_iz9Axyh@O+yCEj1_E{+C9 z6r3nLqNq(LmO2r=BBB}*i4RFs&kUaDvy(_sU*K*=teuuH$jZuR-Gmum`Z>tYL#MgR@RAS!iqk+k*Ri6>po^ zD8YSxz1#5NK2Jeo6ES=a8lOPT0DcqLpK&^ifJx|p*-1d<7G1k;1HYxOU3yw+WZLTB zv^(8tclxLKBh#)APAhb$6=)hW;H01cK)ikX_H9|<$AU-zfM)}W%9jAJ{VHto?HUulyZtx{8{AWJJ5(y%ZY$x-ZO>I_B5=Ny+cNUD z8{v7&Wll8`tj@Qk7l~F3cf7FKYzM+~l?Vf;Ro&TYovTX#aNl>ERpvgJ1po}-!ebLa zdz{f8H|S%hgOTD;StS9$ca;e4=>V|J121Hp+BSvOdrUtb8#9P;*yx}3$pQezbt=M0 z_ss!7lS}E=wW9=zc+KY?W=De-aS6_w)zEjCtK6z9WAMU&MNFl_B@Nsrmw5od)fr6p zRx1isNVF~2J9F!t9He~7^FX<8=#@}A3bQSFBwSzog)^sBmH#vWto$Uqn}0X0%!zVU zTOR44a#yWIUj*EZoY&kBJ)zF?#=2QBb9pQ42KBbzKVs6zl9guMSFY0(zAt1xv2GTU zxm#9JlI8r&x>;D*S>75Tid&n1GpwW@$jw`GPs(4FkDryZkjq&M&vr|vrShS(cU(Rw z9cGPO%9^!{yZbM)CSvU;<#MTfm`&uCtn8Q0L%XzpSac3{pFBEs?qz4j^zLM5bC<15 zOyU%6``FGVH~(!~nY}QmiTYmHY`Ou4~w0Pu1{B`KJ zu^jXB>t#s@YPrKKjg&m>U{?PPgq_AT?T+xu5=i$xouxLlzQSH>`7n9qc(wy%!77Qw{TcUhrdxgkRlAo z$zIEmUfAN4hhe3WH%2@khLG`o9g-i9%;_JdTxo?GQ+#?rVCDrLb)@q>PbHBBTq@2e zGO5SAfl5kEH&W(SeH~WMfD^evoogS$fQL0HaBXPXfD};y##$to@l@>>s;zjW4<#m| zL?4PxL@_FXu9V3uBb42tew>($6a6?g8K-%>VmJoZ9TIp1>pIoSIgY$L)Q=lS9^;t6 zIwf#dSjTwK9TIpkP$*dAG*@npq--2a%;WqOfH>DOj?)ZT z+vkp>uo6e_F?fe6e2TH-jai=So%xzaC@+o(YN0|Mcd8^c^)RgaT;Hjx?V3#zjSJyi z-1BS_Xx#Hoe9^P1;s|ze)3ZqxP_vz(tDa4@gmd<_TGBhB99AVQKM+3O?_DhIBD#;^ z*}N{K;?dwZO(_QL=EW2`#>!#9)kgp{_7u;;J^)IEb8@0LK=xDdRuT%B3_~>}Abo!#(iTbL~ zE2nx+7XkY&CIYLAVu+VOi)Aok;l#!#} zcl(<*wXKwyN%U;R*78Qq$QfxPNj8UD>47obN}EYDX{KVEXeVi;E`0pI?0y&LG{y$w g7{@?SQ7k%F?-JG6xrwZiGBWrx^G4E0Cx-w24~^R{ssI20 literal 0 HcmV?d00001 diff --git a/utils/catkin_ws/src/joypad_bridge/scripts/proto_transmit.py b/utils/catkin_ws/src/joypad_bridge/scripts/proto_transmit.py new file mode 100644 index 0000000..7d1ce00 --- /dev/null +++ b/utils/catkin_ws/src/joypad_bridge/scripts/proto_transmit.py @@ -0,0 +1,36 @@ +import time, serial, struct +import otto_communication_pb2 +from serial import SerialException + +ser = serial.Serial( + baudrate=115200, + parity=serial.PARITY_NONE, + stopbits=serial.STOPBITS_ONE, + bytesize=serial.EIGHTBITS, + rtscts=False, + exclusive=None) +while (ser.is_open == False): + try: + ser.port = '/dev/ttyUSB0' + ser.open() + except SerialException: + print("couldn't open ttyUSB0, check the connection") + time.sleep(2) + +print("open port") + +my_velocity = otto_communication_pb2.VelocityCommand() +my_velocity.linear_velocity = 0.3 +my_velocity.angular_velocity = 0.0 +encode_buffer = my_velocity.SerializeToString() +print(encode_buffer) +print(len(encode_buffer)) + +while 1: + my_velocity.linear_velocity = my_velocity.linear_velocity + 0.01 + encode_buffer = my_velocity.SerializeToString() + print(my_velocity) + print(encode_buffer) + ser.write(encode_buffer) + ser.reset_output_buffer() + time.sleep(0.5) diff --git a/utils/catkin_ws/src/joypad_bridge/scripts/velocities_pb2.py b/utils/catkin_ws/src/joypad_bridge/scripts/velocities_pb2.py deleted file mode 100644 index 2f13360..0000000 --- a/utils/catkin_ws/src/joypad_bridge/scripts/velocities_pb2.py +++ /dev/null @@ -1,76 +0,0 @@ -# Generated by the protocol buffer compiler. DO NOT EDIT! -# source: velocities.proto - -import sys -_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) -from google.protobuf import descriptor as _descriptor -from google.protobuf import message as _message -from google.protobuf import reflection as _reflection -from google.protobuf import symbol_database as _symbol_database -from google.protobuf import descriptor_pb2 -# @@protoc_insertion_point(imports) - -_sym_db = _symbol_database.Default() - - - - -DESCRIPTOR = _descriptor.FileDescriptor( - name='velocities.proto', - package='', - syntax='proto3', - serialized_pb=_b('\n\x10velocities.proto\"5\n\nVelocities\x12\x12\n\nlinear_vel\x18\x01 \x01(\x02\x12\x13\n\x0b\x61ngular_vel\x18\x02 \x01(\x02\x62\x06proto3') -) -_sym_db.RegisterFileDescriptor(DESCRIPTOR) - - - - -_VELOCITIES = _descriptor.Descriptor( - name='Velocities', - full_name='Velocities', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='linear_vel', full_name='Velocities.linear_vel', index=0, - number=1, type=2, cpp_type=6, label=1, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - options=None), - _descriptor.FieldDescriptor( - name='angular_vel', full_name='Velocities.angular_vel', index=1, - number=2, type=2, cpp_type=6, label=1, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - options=None), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - options=None, - is_extendable=False, - syntax='proto3', - extension_ranges=[], - oneofs=[ - ], - serialized_start=20, - serialized_end=73, -) - -DESCRIPTOR.message_types_by_name['Velocities'] = _VELOCITIES - -Velocities = _reflection.GeneratedProtocolMessageType('Velocities', (_message.Message,), dict( - DESCRIPTOR = _VELOCITIES, - __module__ = 'velocities_pb2' - # @@protoc_insertion_point(class_scope:Velocities) - )) -_sym_db.RegisterMessage(Velocities) - - -# @@protoc_insertion_point(module_scope) diff --git a/utils/catkin_ws/src/joypad_bridge/scripts/velocities_pb2.pyc b/utils/catkin_ws/src/joypad_bridge/scripts/velocities_pb2.pyc deleted file mode 100644 index 68b9ec6db01ebedde1fb65d9e237fecdb6c4711b..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2459 zcmcgtZEqVz5Pt5QIJM&>uKSj@=?jH|p(Um*pSaq z9U8$y{D7dDb?l^{0b5?}%+Ag{^UTb;^7mT(=fAdp$uRre1@H|3_gxHVKu54Z z4U)mwD5RmoPMYapUk4A(FPdh4KN3^$-xp&QU^(Cg3}&`s#GpcuM^X@n`3dQH$ehG#)d z)+pOJdv!!f3 z%Ws3sO7|0h&g)`CoxV0!bzYRkkTUN&h4)2g;*E8kO!+q^PkwV9S7x1IaX3~wNlT*# z)Nxs9#@=-f$QGILhFmgE?{AL}{TTp{tQw_SJzfUjIo=#;3f3ITgtpC z@?f;Vlpl`L!X{dIl`1zgUO(lLDfC>zT#d*z0W9d<9+!nLB-~hKZ`6PUnDFBcbFbdY zR;(U)X4g?^lr_JTPR6PG5kS59uD@Fk>-AUj61H0Px-~heGC3f-8QqN9aqC=tRpo<; zohf5hrqx*-W@K^S*8ub<)+U^QbVA=)5`oiVLAc0@+$)ote31CVF$wa%A*-EuTwrh) zP6gOroOrH{$u)(%^T{Zs(s0ov6uUKCIYE()#}deJH_1(_QnLOmfY-_;n)+&DeR81e zgoM1)n#Q8Y0Wc>@`D7F%J~l3)x1Mrmid=G>ONq;hF-hf$G53V?s{l@r>Mown^2Cww zF?Z!8R8G#5me7H(IT9z>kD+LlTJqdRa|cPO@&P#^yvV8OJ2^kmD$c9YoAb;QH|L2H z%C*^LJs17yv;8OC-4`#PcSTH+QK2W6l0glB4mG^_mN)}_+l^XIZlg#F>%QG0v7K7Ji3rQ|{XY#sSJSoT1P5yx->A(Q+fJ4@G&=+_S-A+(CIte)h7-)qa9@($*yk;