From: Federica Di Lauro Date: Tue, 25 Feb 2020 09:35:40 +0000 (+0100) Subject: change status field in StatusMessage to fixed size X-Git-Url: http://git.leonardobizzoni.com/?a=commitdiff_plain;h=998acaaba15376f47962e568f8a39153f74d4abd;p=pioneer-stm32 change status field in StatusMessage to fixed size --- diff --git a/otto_controller/Core/Src/main.cpp b/otto_controller/Core/Src/main.cpp index c06dc10..bd62e03 100644 --- a/otto_controller/Core/Src/main.cpp +++ b/otto_controller/Core/Src/main.cpp @@ -121,6 +121,11 @@ size_t status_msg_length; bool tx_status; float previous_tx_millis; +//TEST variables +long start_time; +long end_time; +int time_diff; + /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ @@ -211,6 +216,9 @@ int main(void) pb_encode(&out_pb_stream, StatusMessage_fields, &status_msg); status_msg_length = out_pb_stream.bytes_written; + //Enables TIM6 interrupt (used for periodic transmission of the odometry) + HAL_TIM_Base_Start_IT(&htim6); + //Enables UART RX interrupt HAL_UART_Receive_DMA(&huart6, (uint8_t*) &proto_buffer_rx, velocity_cmd_length); @@ -313,14 +321,15 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { right_dutycycle -= cross_dutycycle; } - //TIMER 2Hz Transmit + //TIMER 10Hz Transmit if (htim->Instance == TIM6) { - //TODO odometry - pb_ostream_t stream = pb_ostream_from_buffer(proto_buffer_tx, sizeof(proto_buffer_tx)); - odom.FromWheelVelToOdom(left_velocity, right_velocity); + float left_wheel = left_encoder.GetLinearVelocity(); + float right_wheel = right_encoder.GetLinearVelocity(); + + odom.FromWheelVelToOdom(1, -1); status_msg.linear_velocity = odom.GetLinearVelocity(); status_msg.angular_velocity = odom.GetAngularVelocity(); @@ -334,13 +343,20 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { pb_encode(&stream, StatusMessage_fields, &status_msg); HAL_UART_Transmit_DMA(&huart6,(uint8_t*) &proto_buffer_tx, status_msg_length); -// HAL_UART_Transmit(&huart6,(uint8_t*) &proto_buffer_tx, status_msg_length, 100); } } void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) { - mode++; + start_time = HAL_GetTick(); + +// size_t buffer_size = sizeof(proto_buffer_rx); +// uint8_t buffer_copy[buffer_size]; +// memcpy((void *) &buffer_copy, &proto_buffer_rx, buffer_size); +// +// HAL_UART_Receive_DMA(&huart6, (uint8_t*) &proto_buffer_rx, +// velocity_cmd_length); +// mode++; float linear_velocity; float angular_velocity; @@ -359,15 +375,24 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) { 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); + + left_pid.set(0); + right_pid.set(0); float cross_setpoint = left_setpoint - right_setpoint; - cross_pid.set(cross_setpoint); +// cross_pid.set(cross_setpoint); + + cross_pid.set(0); + } HAL_UART_Receive_DMA(&huart6, (uint8_t*) &proto_buffer_rx, velocity_cmd_length); + + end_time = HAL_GetTick(); + time_diff = end_time - start_time; } void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { @@ -377,8 +402,6 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { mode = 1; //Enables TIM3 interrupt (used for PID control) HAL_TIM_Base_Start_IT(&htim3); - //Enables TIM6 interrupt (used for periodic transmission) - HAL_TIM_Base_Start_IT(&htim6); } diff --git a/utils/catkin_ws/dario.launch b/utils/catkin_ws/dario.launch index 0ced7a5..683da0f 100644 --- a/utils/catkin_ws/dario.launch +++ b/utils/catkin_ws/dario.launch @@ -3,4 +3,5 @@ + diff --git a/utils/catkin_ws/joypad.launch b/utils/catkin_ws/joypad.launch index e3765a8..ae40b1e 100644 --- a/utils/catkin_ws/joypad.launch +++ b/utils/catkin_ws/joypad.launch @@ -2,4 +2,5 @@ + 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 21ac630..4724092 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 @@ -6,7 +6,7 @@ from geometry_msgs.msg import Twist from serial import SerialException ser = serial.Serial( - baudrate=115200, + baudrate=9600, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS, @@ -21,9 +21,10 @@ def callback(data): my_velocity.angular_velocity = angular; rospy.loginfo('I heard %f %f', linear, angular) out_buffer = my_velocity.SerializeToString() + print(len(out_buffer)) ser.write(out_buffer) ser.reset_output_buffer() - time.sleep(0.001) + time.sleep(0.002) def listener(): while(ser.is_open == False): diff --git a/utils/catkin_ws/src/joypad_bridge/scripts/cmd_vel_transmitter_backup.py b/utils/catkin_ws/src/joypad_bridge/scripts/cmd_vel_transmitter_backup.py deleted file mode 100755 index fe9a4a2..0000000 --- a/utils/catkin_ws/src/joypad_bridge/scripts/cmd_vel_transmitter_backup.py +++ /dev/null @@ -1,42 +0,0 @@ -#!/usr/bin/env python - -import rospy, serial, struct, time -from geometry_msgs.msg import Twist -from serial import SerialException - -ser = serial.Serial( - baudrate=115200, - parity=serial.PARITY_NONE, - stopbits=serial.STOPBITS_ONE, - bytesize=serial.EIGHTBITS, - rtscts=False) - -def callback(data): - linear = -data.linear.x - angular = -data.angular.z #da fixare? - rospy.loginfo('I heard %f %f', linear, angular) - msg_output = struct.pack(' 0): - radius = lin_vel / ang_vel - icc_x = (odom_values[0] -radius*sin(odom_values[2])) - icc_y = (odom_values[1] + radius*cos(odom_values[2])) - rotation_matrix = numpy.array([[cos(ang_vel*d_time), -sin(ang_vel*d_time), 0], [sin(ang_vel*d_time), cos(ang_vel*d_time), 0], [0, 0, 1]]) - translate_icc_matrix = numpy.array([odom_values[0] - icc_x, odom_values[1] - icc_y, odom_values[2]]) - translate_back_matrix = numpy.array([icc_x, icc_y, ang_vel*d_time]) - odom_values = rotation_matrix.dot(translate_icc_matrix) + translate_back_matrix - elif(d_time > 0): - odom_values = numpy.array([odom_values[0]+(lin_vel/d_time), odom_values[1]+(lin_vel/d_time), odom_values[2]]) - - print(odom_values) - + if (otto_status.status == otto_communication_pb2.StatusMessage.Status.RUNNING): + lin_vel = otto_status.linear_velocity + ang_vel = otto_status.angular_velocity + d_time = otto_status.delta_millis + d_time = d_time / 1000.0 + if(ang_vel != 0 and d_time > 0): + radius = lin_vel / ang_vel + icc_x = (odom_values[0] -radius*sin(odom_values[2])) + icc_y = (odom_values[1] + radius*cos(odom_values[2])) + + rotation_matrix = numpy.array([[cos(ang_vel*d_time), -sin(ang_vel*d_time), 0], + [sin(ang_vel*d_time), cos(ang_vel*d_time), 0], + [0, 0, 1]]) + + translate_icc_matrix = numpy.array([odom_values[0] - icc_x, + odom_values[1] - icc_y, + odom_values[2]]) + + translate_back_matrix = numpy.array([icc_x, + icc_y, + ang_vel*d_time]) + + odom_values = rotation_matrix.dot(translate_icc_matrix) + translate_back_matrix + + elif(d_time > 0): + odom_values = numpy.array([odom_values[0]+cos(odom_values[2])*(lin_vel/d_time), + odom_values[1]+sin(odom_values[2])*(lin_vel/d_time), + odom_values[2]]) + + current_time = rospy.Time.now() + odom_quat = tf.transformations.quaternion_from_euler(0, 0, odom_values[2]) + odom_broadcaster.sendTransform((odom_values[0], odom_values[1], 0.), odom_quat, current_time, "base_link", "odom") + odom = Odometry() + odom.header.stamp = current_time + odom.header.frame_id = "odom" + + odom.pose.pose = Pose(Point(odom_values[0], odom_values[1], 0.), Quaternion(*odom_quat)) + + odom.child_frame_id = "base_link" + odom.twist.twist = Twist(Vector3(lin_vel, 0, 0), Vector3(0, 0, ang_vel)) + + odom_pub.publish(odom) + except DecodeError: print("Decode Error") - + ser.reset_input_buffer() + + end = datetime.datetime.now() + delta = end - start + delta = (delta.total_seconds()*1000) + print(delta) + if __name__ == '__main__': serial_receiver() diff --git a/utils/protobuf_messages/c_protobuf_includes/config_command.pb.c b/utils/protobuf_messages/c_protobuf_includes/config_command.pb.c deleted file mode 100644 index 41fce3c..0000000 --- a/utils/protobuf_messages/c_protobuf_includes/config_command.pb.c +++ /dev/null @@ -1,12 +0,0 @@ -/* Automatically generated nanopb constant definitions */ -/* Generated by 0.4.1-dev */ - -#include "config_command.pb.h" -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -PB_BIND(ConfigCommand, ConfigCommand, AUTO) - - - diff --git a/utils/protobuf_messages/c_protobuf_includes/config_command.pb.h b/utils/protobuf_messages/c_protobuf_includes/config_command.pb.h deleted file mode 100644 index d85af4a..0000000 --- a/utils/protobuf_messages/c_protobuf_includes/config_command.pb.h +++ /dev/null @@ -1,83 +0,0 @@ -/* Automatically generated nanopb header */ -/* Generated by 0.4.1-dev */ - -#ifndef PB_CONFIG_COMMAND_PB_H_INCLUDED -#define PB_CONFIG_COMMAND_PB_H_INCLUDED -#include - -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -/* Struct definitions */ -typedef struct _ConfigCommand { - float left_kp; - float left_ki; - float left_kd; - float right_kp; - float right_ki; - float right_kd; - float cross_kp; - float cross_ki; - float cross_kd; - float baseline; - uint32_t ticks_per_revolution; - float right_wheel_circumference; - float left_wheel_circumference; -} ConfigCommand; - - -/* Initializer values for message structs */ -#define ConfigCommand_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} -#define ConfigCommand_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} - -/* Field tags (for use in manual encoding/decoding) */ -#define ConfigCommand_left_kp_tag 1 -#define ConfigCommand_left_ki_tag 2 -#define ConfigCommand_left_kd_tag 3 -#define ConfigCommand_right_kp_tag 4 -#define ConfigCommand_right_ki_tag 5 -#define ConfigCommand_right_kd_tag 6 -#define ConfigCommand_cross_kp_tag 7 -#define ConfigCommand_cross_ki_tag 8 -#define ConfigCommand_cross_kd_tag 9 -#define ConfigCommand_baseline_tag 10 -#define ConfigCommand_ticks_per_revolution_tag 11 -#define ConfigCommand_right_wheel_circumference_tag 12 -#define ConfigCommand_left_wheel_circumference_tag 13 - -/* Struct field encoding specification for nanopb */ -#define ConfigCommand_FIELDLIST(X, a) \ -X(a, STATIC, SINGULAR, FLOAT, left_kp, 1) \ -X(a, STATIC, SINGULAR, FLOAT, left_ki, 2) \ -X(a, STATIC, SINGULAR, FLOAT, left_kd, 3) \ -X(a, STATIC, SINGULAR, FLOAT, right_kp, 4) \ -X(a, STATIC, SINGULAR, FLOAT, right_ki, 5) \ -X(a, STATIC, SINGULAR, FLOAT, right_kd, 6) \ -X(a, STATIC, SINGULAR, FLOAT, cross_kp, 7) \ -X(a, STATIC, SINGULAR, FLOAT, cross_ki, 8) \ -X(a, STATIC, SINGULAR, FLOAT, cross_kd, 9) \ -X(a, STATIC, SINGULAR, FLOAT, baseline, 10) \ -X(a, STATIC, SINGULAR, FIXED32, ticks_per_revolution, 11) \ -X(a, STATIC, SINGULAR, FLOAT, right_wheel_circumference, 12) \ -X(a, STATIC, SINGULAR, FLOAT, left_wheel_circumference, 13) -#define ConfigCommand_CALLBACK NULL -#define ConfigCommand_DEFAULT NULL - -extern const pb_msgdesc_t ConfigCommand_msg; - -/* Defines for backwards compatibility with code written before nanopb-0.4.0 */ -#define ConfigCommand_fields &ConfigCommand_msg - -/* Maximum encoded size of messages (where known) */ -#define ConfigCommand_size 65 - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif diff --git a/utils/protobuf_messages/c_protobuf_includes/status_message.pb.c b/utils/protobuf_messages/c_protobuf_includes/status_message.pb.c deleted file mode 100644 index 2367ff3..0000000 --- a/utils/protobuf_messages/c_protobuf_includes/status_message.pb.c +++ /dev/null @@ -1,13 +0,0 @@ -/* Automatically generated nanopb constant definitions */ -/* Generated by 0.4.1-dev */ - -#include "status_message.pb.h" -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -PB_BIND(VelocityCommand, VelocityCommand, AUTO) - - - - diff --git a/utils/protobuf_messages/c_protobuf_includes/status_message.pb.h b/utils/protobuf_messages/c_protobuf_includes/status_message.pb.h deleted file mode 100644 index ab04922..0000000 --- a/utils/protobuf_messages/c_protobuf_includes/status_message.pb.h +++ /dev/null @@ -1,73 +0,0 @@ -/* Automatically generated nanopb header */ -/* Generated by 0.4.1-dev */ - -#ifndef PB_STATUS_MESSAGE_PB_H_INCLUDED -#define PB_STATUS_MESSAGE_PB_H_INCLUDED -#include - -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -/* Enum definitions */ -typedef enum _VelocityCommand_Status { - VelocityCommand_Status_UNKNOWN = 0, - VelocityCommand_Status_WAITING_CONFIG = 1, - VelocityCommand_Status_READY = 2, - VelocityCommand_Status_RUNNING = 3, - VelocityCommand_Status_H_BRIDGE_FAULT_1 = 4, - VelocityCommand_Status_H_BRIDGE_FAULT_2 = 5, - VelocityCommand_Status_UNKNOWN_ERROR = 6 -} VelocityCommand_Status; - -/* Struct definitions */ -typedef struct _VelocityCommand { - float linear_velocity; - float angular_velocity; - uint64_t delta_millis; - VelocityCommand_Status status; -} VelocityCommand; - - -/* Helper constants for enums */ -#define _VelocityCommand_Status_MIN VelocityCommand_Status_UNKNOWN -#define _VelocityCommand_Status_MAX VelocityCommand_Status_UNKNOWN_ERROR -#define _VelocityCommand_Status_ARRAYSIZE ((VelocityCommand_Status)(VelocityCommand_Status_UNKNOWN_ERROR+1)) - - -/* Initializer values for message structs */ -#define VelocityCommand_init_default {0, 0, 0, _VelocityCommand_Status_MIN} -#define VelocityCommand_init_zero {0, 0, 0, _VelocityCommand_Status_MIN} - -/* Field tags (for use in manual encoding/decoding) */ -#define VelocityCommand_linear_velocity_tag 1 -#define VelocityCommand_angular_velocity_tag 2 -#define VelocityCommand_delta_millis_tag 3 -#define VelocityCommand_status_tag 4 - -/* Struct field encoding specification for nanopb */ -#define VelocityCommand_FIELDLIST(X, a) \ -X(a, STATIC, SINGULAR, FLOAT, linear_velocity, 1) \ -X(a, STATIC, SINGULAR, FLOAT, angular_velocity, 2) \ -X(a, STATIC, SINGULAR, FIXED64, delta_millis, 3) \ -X(a, STATIC, SINGULAR, UENUM, status, 4) -#define VelocityCommand_CALLBACK NULL -#define VelocityCommand_DEFAULT NULL - -extern const pb_msgdesc_t VelocityCommand_msg; - -/* Defines for backwards compatibility with code written before nanopb-0.4.0 */ -#define VelocityCommand_fields &VelocityCommand_msg - -/* Maximum encoded size of messages (where known) */ -#define VelocityCommand_size 21 - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif diff --git a/utils/protobuf_messages/c_protobuf_includes/velocity_command.pb.c b/utils/protobuf_messages/c_protobuf_includes/velocity_command.pb.c deleted file mode 100644 index 1f43d4f..0000000 --- a/utils/protobuf_messages/c_protobuf_includes/velocity_command.pb.c +++ /dev/null @@ -1,12 +0,0 @@ -/* Automatically generated nanopb constant definitions */ -/* Generated by 0.4.1-dev */ - -#include "velocity_command.pb.h" -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -PB_BIND(VelocityCommand, VelocityCommand, AUTO) - - - diff --git a/utils/protobuf_messages/c_protobuf_includes/velocity_command.pb.h b/utils/protobuf_messages/c_protobuf_includes/velocity_command.pb.h deleted file mode 100644 index e64811f..0000000 --- a/utils/protobuf_messages/c_protobuf_includes/velocity_command.pb.h +++ /dev/null @@ -1,50 +0,0 @@ -/* Automatically generated nanopb header */ -/* Generated by 0.4.1-dev */ - -#ifndef PB_VELOCITY_COMMAND_PB_H_INCLUDED -#define PB_VELOCITY_COMMAND_PB_H_INCLUDED -#include - -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -/* Struct definitions */ -typedef struct _VelocityCommand { - float linear_velocity; - float angular_velocity; -} VelocityCommand; - - -/* Initializer values for message structs */ -#define VelocityCommand_init_default {0, 0} -#define VelocityCommand_init_zero {0, 0} - -/* Field tags (for use in manual encoding/decoding) */ -#define VelocityCommand_linear_velocity_tag 1 -#define VelocityCommand_angular_velocity_tag 2 - -/* Struct field encoding specification for nanopb */ -#define VelocityCommand_FIELDLIST(X, a) \ -X(a, STATIC, SINGULAR, FLOAT, linear_velocity, 1) \ -X(a, STATIC, SINGULAR, FLOAT, angular_velocity, 2) -#define VelocityCommand_CALLBACK NULL -#define VelocityCommand_DEFAULT NULL - -extern const pb_msgdesc_t VelocityCommand_msg; - -/* Defines for backwards compatibility with code written before nanopb-0.4.0 */ -#define VelocityCommand_fields &VelocityCommand_msg - -/* Maximum encoded size of messages (where known) */ -#define VelocityCommand_size 10 - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif diff --git a/utils/protobuf_messages/messages_definitions/config_command.proto b/utils/protobuf_messages/messages_definitions/config_command.proto deleted file mode 100644 index 8a43c6e..0000000 --- a/utils/protobuf_messages/messages_definitions/config_command.proto +++ /dev/null @@ -1,21 +0,0 @@ -syntax = "proto3"; - -message ConfigCommand { - float left_kp = 1; - float left_ki = 2; - float left_kd = 3; - - float right_kp = 4; - float right_ki = 5; - float right_kd = 6; - - float cross_kp = 7; - float cross_ki = 8; - float cross_kd = 9; - - float baseline = 10; - - fixed32 ticks_per_revolution = 11; - float right_wheel_circumference = 12; - float left_wheel_circumference = 13; -} \ No newline at end of file diff --git a/utils/protobuf_messages/messages_definitions/status_message.proto b/utils/protobuf_messages/messages_definitions/status_message.proto deleted file mode 100644 index b259cee..0000000 --- a/utils/protobuf_messages/messages_definitions/status_message.proto +++ /dev/null @@ -1,17 +0,0 @@ -syntax = "proto3"; - -message StatusMessage { - required float linear_velocity = 1; - required float angular_velocity = 2; - required fixed64 delta_millis = 3; - enum Status { - UNKNOWN = 0; - WAITING_CONFIG = 1; - READY = 2; - RUNNING = 3; - H_BRIDGE_FAULT_1 = 4; - H_BRIDGE_FAULT_2 = 5; - UNKNOWN_ERROR = 6; - } - required Status status = 4; -} \ No newline at end of file diff --git a/utils/protobuf_messages/messages_definitions/velocity_command.proto b/utils/protobuf_messages/messages_definitions/velocity_command.proto deleted file mode 100644 index afd3f74..0000000 --- a/utils/protobuf_messages/messages_definitions/velocity_command.proto +++ /dev/null @@ -1,6 +0,0 @@ -syntax = "proto3"; - -message VelocityCommand { - float linear_velocity = 1; - float angular_velocity = 2; -} \ No newline at end of file diff --git a/utils/protobuf_messages/messages_definitions/otto_communication.pb b/utils/protobuf_messages/otto_communication.pb similarity index 72% rename from utils/protobuf_messages/messages_definitions/otto_communication.pb rename to utils/protobuf_messages/otto_communication.pb index 5151120..435262e 100644 Binary files a/utils/protobuf_messages/messages_definitions/otto_communication.pb and b/utils/protobuf_messages/otto_communication.pb differ diff --git a/utils/protobuf_messages/messages_definitions/otto_communication.pb.c b/utils/protobuf_messages/otto_communication.pb.c similarity index 99% rename from utils/protobuf_messages/messages_definitions/otto_communication.pb.c rename to utils/protobuf_messages/otto_communication.pb.c index 9a82e27..79705b7 100644 --- a/utils/protobuf_messages/messages_definitions/otto_communication.pb.c +++ b/utils/protobuf_messages/otto_communication.pb.c @@ -16,4 +16,3 @@ PB_BIND(VelocityCommand, VelocityCommand, AUTO) - diff --git a/utils/protobuf_messages/messages_definitions/otto_communication.pb.h b/utils/protobuf_messages/otto_communication.pb.h similarity index 80% rename from utils/protobuf_messages/messages_definitions/otto_communication.pb.h rename to utils/protobuf_messages/otto_communication.pb.h index 8fca389..0822d21 100644 --- a/utils/protobuf_messages/messages_definitions/otto_communication.pb.h +++ b/utils/protobuf_messages/otto_communication.pb.h @@ -13,17 +13,6 @@ extern "C" { #endif -/* Enum definitions */ -typedef enum _StatusMessage_Status { - StatusMessage_Status_UNKNOWN = 0, - StatusMessage_Status_WAITING_CONFIG = 1, - StatusMessage_Status_READY = 2, - StatusMessage_Status_RUNNING = 3, - StatusMessage_Status_H_BRIDGE_FAULT_1 = 4, - StatusMessage_Status_H_BRIDGE_FAULT_2 = 5, - StatusMessage_Status_UNKNOWN_ERROR = 6 -} StatusMessage_Status; - /* Struct definitions */ typedef struct _ConfigCommand { float left_kp; @@ -44,8 +33,8 @@ typedef struct _ConfigCommand { typedef struct _StatusMessage { float linear_velocity; float angular_velocity; - uint64_t delta_millis; - StatusMessage_Status status; + uint32_t delta_millis; + uint32_t status; } StatusMessage; typedef struct _VelocityCommand { @@ -54,17 +43,11 @@ typedef struct _VelocityCommand { } VelocityCommand; -/* Helper constants for enums */ -#define _StatusMessage_Status_MIN StatusMessage_Status_UNKNOWN -#define _StatusMessage_Status_MAX StatusMessage_Status_UNKNOWN_ERROR -#define _StatusMessage_Status_ARRAYSIZE ((StatusMessage_Status)(StatusMessage_Status_UNKNOWN_ERROR+1)) - - /* Initializer values for message structs */ -#define StatusMessage_init_default {0, 0, 0, _StatusMessage_Status_MIN} +#define StatusMessage_init_default {0, 0, 0, 0} #define ConfigCommand_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} #define VelocityCommand_init_default {0, 0} -#define StatusMessage_init_zero {0, 0, 0, _StatusMessage_Status_MIN} +#define StatusMessage_init_zero {0, 0, 0, 0} #define ConfigCommand_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} #define VelocityCommand_init_zero {0, 0} @@ -93,8 +76,8 @@ typedef struct _VelocityCommand { #define StatusMessage_FIELDLIST(X, a) \ X(a, STATIC, REQUIRED, FLOAT, linear_velocity, 1) \ X(a, STATIC, REQUIRED, FLOAT, angular_velocity, 2) \ -X(a, STATIC, REQUIRED, FIXED64, delta_millis, 3) \ -X(a, STATIC, REQUIRED, UENUM, status, 4) +X(a, STATIC, REQUIRED, FIXED32, delta_millis, 3) \ +X(a, STATIC, REQUIRED, FIXED32, status, 4) #define StatusMessage_CALLBACK NULL #define StatusMessage_DEFAULT NULL @@ -131,7 +114,7 @@ extern const pb_msgdesc_t VelocityCommand_msg; #define VelocityCommand_fields &VelocityCommand_msg /* Maximum encoded size of messages (where known) */ -#define StatusMessage_size 21 +#define StatusMessage_size 20 #define ConfigCommand_size 65 #define VelocityCommand_size 10 diff --git a/utils/protobuf_messages/messages_definitions/otto_communication.proto b/utils/protobuf_messages/otto_communication.proto similarity index 65% rename from utils/protobuf_messages/messages_definitions/otto_communication.proto rename to utils/protobuf_messages/otto_communication.proto index 5ec2278..7568b4c 100644 --- a/utils/protobuf_messages/messages_definitions/otto_communication.proto +++ b/utils/protobuf_messages/otto_communication.proto @@ -1,19 +1,24 @@ syntax = "proto2"; - + +// Message sent from ST board to the computer message StatusMessage { required float linear_velocity = 1; required float angular_velocity = 2; - required fixed64 delta_millis = 3; - enum Status { - UNKNOWN = 0; - WAITING_CONFIG = 1; - READY = 2; - RUNNING = 3; - H_BRIDGE_FAULT_1 = 4; - H_BRIDGE_FAULT_2 = 5; - UNKNOWN_ERROR = 6; - } - required Status status = 4; + required fixed32 delta_millis = 3; + /* + Status code meanings: + 0 = UNKNOWN + 1 = WAITING_CONFIG + 2 = READY + 3 = RUNNING + 4 = H_BRIDGE_FAULT_1 + 5 = H_BRIDGE_FAULT_2 + 6 = UNKNOWN_ERROR + + Note that enum is not used because it doesn't have a + fixed size + */ + required fixed32 status = 4; } message ConfigCommand { diff --git a/utils/protobuf_messages/messages_definitions/otto_communication_pb2.py b/utils/protobuf_messages/otto_communication_pb2.py similarity index 76% rename from utils/protobuf_messages/messages_definitions/otto_communication_pb2.py rename to utils/protobuf_messages/otto_communication_pb2.py index fe68e9f..340dd63 100644 --- a/utils/protobuf_messages/messages_definitions/otto_communication_pb2.py +++ b/utils/protobuf_messages/otto_communication_pb2.py @@ -19,54 +19,12 @@ DESCRIPTOR = _descriptor.FileDescriptor( name='otto_communication.proto', package='', syntax='proto2', - serialized_pb=_b('\n\x18otto_communication.proto\"\x82\x02\n\rStatusMessage\x12\x17\n\x0flinear_velocity\x18\x01 \x02(\x02\x12\x18\n\x10\x61ngular_velocity\x18\x02 \x02(\x02\x12\x14\n\x0c\x64\x65lta_millis\x18\x03 \x02(\x06\x12%\n\x06status\x18\x04 \x02(\x0e\x32\x15.StatusMessage.Status\"\x80\x01\n\x06Status\x12\x0b\n\x07UNKNOWN\x10\x00\x12\x12\n\x0eWAITING_CONFIG\x10\x01\x12\t\n\x05READY\x10\x02\x12\x0b\n\x07RUNNING\x10\x03\x12\x14\n\x10H_BRIDGE_FAULT_1\x10\x04\x12\x14\n\x10H_BRIDGE_FAULT_2\x10\x05\x12\x11\n\rUNKNOWN_ERROR\x10\x06\"\xa3\x02\n\rConfigCommand\x12\x0f\n\x07left_kp\x18\x01 \x02(\x02\x12\x0f\n\x07left_ki\x18\x02 \x02(\x02\x12\x0f\n\x07left_kd\x18\x03 \x02(\x02\x12\x10\n\x08right_kp\x18\x04 \x02(\x02\x12\x10\n\x08right_ki\x18\x05 \x02(\x02\x12\x10\n\x08right_kd\x18\x06 \x02(\x02\x12\x10\n\x08\x63ross_kp\x18\x07 \x02(\x02\x12\x10\n\x08\x63ross_ki\x18\x08 \x02(\x02\x12\x10\n\x08\x63ross_kd\x18\t \x02(\x02\x12\x10\n\x08\x62\x61seline\x18\n \x02(\x02\x12\x1c\n\x14ticks_per_revolution\x18\x0b \x02(\x07\x12!\n\x19right_wheel_circumference\x18\x0c \x02(\x02\x12 \n\x18left_wheel_circumference\x18\r \x02(\x02\"D\n\x0fVelocityCommand\x12\x17\n\x0flinear_velocity\x18\x01 \x02(\x02\x12\x18\n\x10\x61ngular_velocity\x18\x02 \x02(\x02') + serialized_pb=_b('\n\x18otto_communication.proto\"h\n\rStatusMessage\x12\x17\n\x0flinear_velocity\x18\x01 \x02(\x02\x12\x18\n\x10\x61ngular_velocity\x18\x02 \x02(\x02\x12\x14\n\x0c\x64\x65lta_millis\x18\x03 \x02(\x07\x12\x0e\n\x06status\x18\x04 \x02(\x07\"\xa3\x02\n\rConfigCommand\x12\x0f\n\x07left_kp\x18\x01 \x02(\x02\x12\x0f\n\x07left_ki\x18\x02 \x02(\x02\x12\x0f\n\x07left_kd\x18\x03 \x02(\x02\x12\x10\n\x08right_kp\x18\x04 \x02(\x02\x12\x10\n\x08right_ki\x18\x05 \x02(\x02\x12\x10\n\x08right_kd\x18\x06 \x02(\x02\x12\x10\n\x08\x63ross_kp\x18\x07 \x02(\x02\x12\x10\n\x08\x63ross_ki\x18\x08 \x02(\x02\x12\x10\n\x08\x63ross_kd\x18\t \x02(\x02\x12\x10\n\x08\x62\x61seline\x18\n \x02(\x02\x12\x1c\n\x14ticks_per_revolution\x18\x0b \x02(\x07\x12!\n\x19right_wheel_circumference\x18\x0c \x02(\x02\x12 \n\x18left_wheel_circumference\x18\r \x02(\x02\"D\n\x0fVelocityCommand\x12\x17\n\x0flinear_velocity\x18\x01 \x02(\x02\x12\x18\n\x10\x61ngular_velocity\x18\x02 \x02(\x02') ) _sym_db.RegisterFileDescriptor(DESCRIPTOR) -_STATUSMESSAGE_STATUS = _descriptor.EnumDescriptor( - name='Status', - full_name='StatusMessage.Status', - filename=None, - file=DESCRIPTOR, - values=[ - _descriptor.EnumValueDescriptor( - name='UNKNOWN', index=0, number=0, - options=None, - type=None), - _descriptor.EnumValueDescriptor( - name='WAITING_CONFIG', index=1, number=1, - options=None, - type=None), - _descriptor.EnumValueDescriptor( - name='READY', index=2, number=2, - options=None, - type=None), - _descriptor.EnumValueDescriptor( - name='RUNNING', index=3, number=3, - options=None, - type=None), - _descriptor.EnumValueDescriptor( - name='H_BRIDGE_FAULT_1', index=4, number=4, - options=None, - type=None), - _descriptor.EnumValueDescriptor( - name='H_BRIDGE_FAULT_2', index=5, number=5, - options=None, - type=None), - _descriptor.EnumValueDescriptor( - name='UNKNOWN_ERROR', index=6, number=6, - options=None, - type=None), - ], - containing_type=None, - options=None, - serialized_start=159, - serialized_end=287, -) -_sym_db.RegisterEnumDescriptor(_STATUSMESSAGE_STATUS) - _STATUSMESSAGE = _descriptor.Descriptor( name='StatusMessage', @@ -91,14 +49,14 @@ _STATUSMESSAGE = _descriptor.Descriptor( options=None), _descriptor.FieldDescriptor( name='delta_millis', full_name='StatusMessage.delta_millis', index=2, - number=3, type=6, cpp_type=4, label=2, + number=3, type=7, cpp_type=3, label=2, has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, options=None), _descriptor.FieldDescriptor( name='status', full_name='StatusMessage.status', index=3, - number=4, type=14, cpp_type=8, label=2, + number=4, type=7, cpp_type=3, label=2, has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, @@ -108,7 +66,6 @@ _STATUSMESSAGE = _descriptor.Descriptor( ], nested_types=[], enum_types=[ - _STATUSMESSAGE_STATUS, ], options=None, is_extendable=False, @@ -116,8 +73,8 @@ _STATUSMESSAGE = _descriptor.Descriptor( extension_ranges=[], oneofs=[ ], - serialized_start=29, - serialized_end=287, + serialized_start=28, + serialized_end=132, ) @@ -231,8 +188,8 @@ _CONFIGCOMMAND = _descriptor.Descriptor( extension_ranges=[], oneofs=[ ], - serialized_start=290, - serialized_end=581, + serialized_start=135, + serialized_end=426, ) @@ -269,12 +226,10 @@ _VELOCITYCOMMAND = _descriptor.Descriptor( extension_ranges=[], oneofs=[ ], - serialized_start=583, - serialized_end=651, + serialized_start=428, + serialized_end=496, ) -_STATUSMESSAGE.fields_by_name['status'].enum_type = _STATUSMESSAGE_STATUS -_STATUSMESSAGE_STATUS.containing_type = _STATUSMESSAGE DESCRIPTOR.message_types_by_name['StatusMessage'] = _STATUSMESSAGE DESCRIPTOR.message_types_by_name['ConfigCommand'] = _CONFIGCOMMAND DESCRIPTOR.message_types_by_name['VelocityCommand'] = _VELOCITYCOMMAND diff --git a/utils/protobuf_messages/python_protobuf_includes/config_command_pb2.py b/utils/protobuf_messages/python_protobuf_includes/config_command_pb2.py deleted file mode 100644 index 58cca1c..0000000 --- a/utils/protobuf_messages/python_protobuf_includes/config_command_pb2.py +++ /dev/null @@ -1,153 +0,0 @@ -# Generated by the protocol buffer compiler. DO NOT EDIT! -# source: config_command.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='config_command.proto', - package='', - syntax='proto3', - serialized_pb=_b('\n\x14\x63onfig_command.proto\"\xa3\x02\n\rConfigCommand\x12\x0f\n\x07left_kp\x18\x01 \x01(\x02\x12\x0f\n\x07left_ki\x18\x02 \x01(\x02\x12\x0f\n\x07left_kd\x18\x03 \x01(\x02\x12\x10\n\x08right_kp\x18\x04 \x01(\x02\x12\x10\n\x08right_ki\x18\x05 \x01(\x02\x12\x10\n\x08right_kd\x18\x06 \x01(\x02\x12\x10\n\x08\x63ross_kp\x18\x07 \x01(\x02\x12\x10\n\x08\x63ross_ki\x18\x08 \x01(\x02\x12\x10\n\x08\x63ross_kd\x18\t \x01(\x02\x12\x10\n\x08\x62\x61seline\x18\n \x01(\x02\x12\x1c\n\x14ticks_per_revolution\x18\x0b \x01(\x07\x12!\n\x19right_wheel_circumference\x18\x0c \x01(\x02\x12 \n\x18left_wheel_circumference\x18\r \x01(\x02\x62\x06proto3') -) -_sym_db.RegisterFileDescriptor(DESCRIPTOR) - - - - -_CONFIGCOMMAND = _descriptor.Descriptor( - name='ConfigCommand', - full_name='ConfigCommand', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='left_kp', full_name='ConfigCommand.left_kp', 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='left_ki', full_name='ConfigCommand.left_ki', 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), - _descriptor.FieldDescriptor( - name='left_kd', full_name='ConfigCommand.left_kd', index=2, - number=3, 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='right_kp', full_name='ConfigCommand.right_kp', index=3, - number=4, 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='right_ki', full_name='ConfigCommand.right_ki', index=4, - number=5, 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='right_kd', full_name='ConfigCommand.right_kd', index=5, - number=6, 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='cross_kp', full_name='ConfigCommand.cross_kp', index=6, - number=7, 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='cross_ki', full_name='ConfigCommand.cross_ki', index=7, - number=8, 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='cross_kd', full_name='ConfigCommand.cross_kd', index=8, - number=9, 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='baseline', full_name='ConfigCommand.baseline', index=9, - number=10, 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='ticks_per_revolution', full_name='ConfigCommand.ticks_per_revolution', index=10, - number=11, type=7, cpp_type=3, label=1, - has_default_value=False, default_value=0, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - options=None), - _descriptor.FieldDescriptor( - name='right_wheel_circumference', full_name='ConfigCommand.right_wheel_circumference', index=11, - number=12, 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='left_wheel_circumference', full_name='ConfigCommand.left_wheel_circumference', index=12, - number=13, 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=25, - serialized_end=316, -) - -DESCRIPTOR.message_types_by_name['ConfigCommand'] = _CONFIGCOMMAND - -ConfigCommand = _reflection.GeneratedProtocolMessageType('ConfigCommand', (_message.Message,), dict( - DESCRIPTOR = _CONFIGCOMMAND, - __module__ = 'config_command_pb2' - # @@protoc_insertion_point(class_scope:ConfigCommand) - )) -_sym_db.RegisterMessage(ConfigCommand) - - -# @@protoc_insertion_point(module_scope) diff --git a/utils/protobuf_messages/python_protobuf_includes/status_message_pb2.py b/utils/protobuf_messages/python_protobuf_includes/status_message_pb2.py deleted file mode 100644 index c5cc154..0000000 --- a/utils/protobuf_messages/python_protobuf_includes/status_message_pb2.py +++ /dev/null @@ -1,135 +0,0 @@ -# Generated by the protocol buffer compiler. DO NOT EDIT! -# source: status_message.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='status_message.proto', - package='', - syntax='proto3', - serialized_pb=_b('\n\x14status_message.proto\"\x86\x02\n\x0fVelocityCommand\x12\x17\n\x0flinear_velocity\x18\x01 \x01(\x02\x12\x18\n\x10\x61ngular_velocity\x18\x02 \x01(\x02\x12\x14\n\x0c\x64\x65lta_millis\x18\x03 \x01(\x06\x12\'\n\x06status\x18\x04 \x01(\x0e\x32\x17.VelocityCommand.Status\"\x80\x01\n\x06Status\x12\x0b\n\x07UNKNOWN\x10\x00\x12\x12\n\x0eWAITING_CONFIG\x10\x01\x12\t\n\x05READY\x10\x02\x12\x0b\n\x07RUNNING\x10\x03\x12\x14\n\x10H_BRIDGE_FAULT_1\x10\x04\x12\x14\n\x10H_BRIDGE_FAULT_2\x10\x05\x12\x11\n\rUNKNOWN_ERROR\x10\x06\x62\x06proto3') -) -_sym_db.RegisterFileDescriptor(DESCRIPTOR) - - - -_VELOCITYCOMMAND_STATUS = _descriptor.EnumDescriptor( - name='Status', - full_name='VelocityCommand.Status', - filename=None, - file=DESCRIPTOR, - values=[ - _descriptor.EnumValueDescriptor( - name='UNKNOWN', index=0, number=0, - options=None, - type=None), - _descriptor.EnumValueDescriptor( - name='WAITING_CONFIG', index=1, number=1, - options=None, - type=None), - _descriptor.EnumValueDescriptor( - name='READY', index=2, number=2, - options=None, - type=None), - _descriptor.EnumValueDescriptor( - name='RUNNING', index=3, number=3, - options=None, - type=None), - _descriptor.EnumValueDescriptor( - name='H_BRIDGE_FAULT_1', index=4, number=4, - options=None, - type=None), - _descriptor.EnumValueDescriptor( - name='H_BRIDGE_FAULT_2', index=5, number=5, - options=None, - type=None), - _descriptor.EnumValueDescriptor( - name='UNKNOWN_ERROR', index=6, number=6, - options=None, - type=None), - ], - containing_type=None, - options=None, - serialized_start=159, - serialized_end=287, -) -_sym_db.RegisterEnumDescriptor(_VELOCITYCOMMAND_STATUS) - - -_VELOCITYCOMMAND = _descriptor.Descriptor( - name='VelocityCommand', - full_name='VelocityCommand', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='linear_velocity', full_name='VelocityCommand.linear_velocity', 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_velocity', full_name='VelocityCommand.angular_velocity', 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), - _descriptor.FieldDescriptor( - name='delta_millis', full_name='VelocityCommand.delta_millis', index=2, - number=3, type=6, cpp_type=4, label=1, - has_default_value=False, default_value=0, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - options=None), - _descriptor.FieldDescriptor( - name='status', full_name='VelocityCommand.status', index=3, - number=4, type=14, cpp_type=8, label=1, - has_default_value=False, default_value=0, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - options=None), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - _VELOCITYCOMMAND_STATUS, - ], - options=None, - is_extendable=False, - syntax='proto3', - extension_ranges=[], - oneofs=[ - ], - serialized_start=25, - serialized_end=287, -) - -_VELOCITYCOMMAND.fields_by_name['status'].enum_type = _VELOCITYCOMMAND_STATUS -_VELOCITYCOMMAND_STATUS.containing_type = _VELOCITYCOMMAND -DESCRIPTOR.message_types_by_name['VelocityCommand'] = _VELOCITYCOMMAND - -VelocityCommand = _reflection.GeneratedProtocolMessageType('VelocityCommand', (_message.Message,), dict( - DESCRIPTOR = _VELOCITYCOMMAND, - __module__ = 'status_message_pb2' - # @@protoc_insertion_point(class_scope:VelocityCommand) - )) -_sym_db.RegisterMessage(VelocityCommand) - - -# @@protoc_insertion_point(module_scope) diff --git a/utils/protobuf_messages/python_protobuf_includes/velocity_command_pb2.py b/utils/protobuf_messages/python_protobuf_includes/velocity_command_pb2.py deleted file mode 100644 index 53fce6f..0000000 --- a/utils/protobuf_messages/python_protobuf_includes/velocity_command_pb2.py +++ /dev/null @@ -1,76 +0,0 @@ -# Generated by the protocol buffer compiler. DO NOT EDIT! -# source: velocity_command.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='velocity_command.proto', - package='', - syntax='proto3', - serialized_pb=_b('\n\x16velocity_command.proto\"D\n\x0fVelocityCommand\x12\x17\n\x0flinear_velocity\x18\x01 \x01(\x02\x12\x18\n\x10\x61ngular_velocity\x18\x02 \x01(\x02\x62\x06proto3') -) -_sym_db.RegisterFileDescriptor(DESCRIPTOR) - - - - -_VELOCITYCOMMAND = _descriptor.Descriptor( - name='VelocityCommand', - full_name='VelocityCommand', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='linear_velocity', full_name='VelocityCommand.linear_velocity', 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_velocity', full_name='VelocityCommand.angular_velocity', 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=26, - serialized_end=94, -) - -DESCRIPTOR.message_types_by_name['VelocityCommand'] = _VELOCITYCOMMAND - -VelocityCommand = _reflection.GeneratedProtocolMessageType('VelocityCommand', (_message.Message,), dict( - DESCRIPTOR = _VELOCITYCOMMAND, - __module__ = 'velocity_command_pb2' - # @@protoc_insertion_point(class_scope:VelocityCommand) - )) -_sym_db.RegisterMessage(VelocityCommand) - - -# @@protoc_insertion_point(module_scope) diff --git a/utils/py_serial_examples/receive.py b/utils/py_serial_examples/receive.py index 9da5f1d..4569e2f 100644 --- a/utils/py_serial_examples/receive.py +++ b/utils/py_serial_examples/receive.py @@ -5,22 +5,23 @@ ser = serial.Serial( parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS, - rtscts=False, + rtscts=True, 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) + try: + ser.port = '/dev/ttyUSB1' + ser.open() + except SerialException: + print("couldn't open ttyUSB1, check the connection") + time.sleep(2) print("port open") - +ser.reset_input_buffer() +ser.rts = False while 1: - ser.reset_input_buffer() - buffer = ser.read(8) - msg_received = struct.unpack('