From e73f48e22d481131f28fb256d72dca9553a65599 Mon Sep 17 00:00:00 2001 From: Federica Di Lauro Date: Wed, 12 Feb 2020 14:09:36 +0100 Subject: [PATCH] receive cmd_vel and send velocity with protobuf --- otto_controller/Core/Inc/odometry.h | 35 ++++++++---- otto_controller/Core/Src/main.cpp | 31 ++++++++--- .../scripts/cmd_vel_transmitter.py | 6 +-- .../joypad_bridge/scripts/proto_transmit.py | 36 ------------- .../joypad_bridge/scripts/serial_receiver.py | 54 +++++++++++++++++++ utils/py_serial_examples/proto_transmit.py | 16 +++--- 6 files changed, 111 insertions(+), 67 deletions(-) delete mode 100644 utils/catkin_ws/src/joypad_bridge/scripts/proto_transmit.py create mode 100755 utils/catkin_ws/src/joypad_bridge/scripts/serial_receiver.py diff --git a/otto_controller/Core/Inc/odometry.h b/otto_controller/Core/Inc/odometry.h index 85f24f7..417f062 100644 --- a/otto_controller/Core/Inc/odometry.h +++ b/otto_controller/Core/Inc/odometry.h @@ -4,33 +4,46 @@ class Odometry { private: - float left_velocity_; - float right_velocity_; + float left_setpoint_; + float right_setpoint_; + float linear_velocity_; + float angular_velocity_; float baseline_; public: Odometry() { - left_velocity_ = 0; - right_velocity_ = 0; + left_setpoint_ = 0; + right_setpoint_ = 0; baseline_ = 0; } Odometry(float baseline) { - left_velocity_ = 0; - right_velocity_ = 0; + left_setpoint_ = 0; + right_setpoint_ = 0; baseline_ = baseline; } - void UpdateValues(float linear_vel, float angular_vel) { - left_velocity_ = linear_vel - (baseline_ * angular_vel) / 2; - right_velocity_ = linear_vel + (baseline_ * angular_vel) / 2; + void FromCmdVelToSetpoint(float linear_vel, float angular_vel) { + left_setpoint_ = linear_vel - (baseline_ * angular_vel) / 2; + right_setpoint_ = linear_vel + (baseline_ * angular_vel) / 2; + } + + void FromWheelVelToOdom(float left_wheel_vel, float right_wheel_vel){ + linear_velocity_ = (left_wheel_vel + right_wheel_vel)/2; + angular_velocity_ = (right_wheel_vel - left_wheel_vel)/baseline_; } float GetLeftVelocity() { - return left_velocity_; + return left_setpoint_; } float GetRightVelocity() { - return right_velocity_; + return right_setpoint_; + } + float GetLinearVelocity(){ + return linear_velocity_; + } + float GetAngularVelocity(){ + return angular_velocity_; } }; diff --git a/otto_controller/Core/Src/main.cpp b/otto_controller/Core/Src/main.cpp index d19ac9f..485ec55 100644 --- a/otto_controller/Core/Src/main.cpp +++ b/otto_controller/Core/Src/main.cpp @@ -119,6 +119,7 @@ pb_ostream_t out_pb_stream; StatusMessage status_msg; size_t status_msg_length; bool tx_status; +float previous_tx_millis; /* USER CODE END PV */ @@ -203,9 +204,11 @@ int main(void) { sizeof(proto_buffer_tx)); pb_encode(&out_pb_stream, VelocityCommand_fields, &vel_cmd); - velocity_cmd_length = out_pb_stream.bytes_written; + pb_encode(&out_pb_stream, StatusMessage_fields, &status_msg); + status_msg_length = out_pb_stream.bytes_written; + //Enables UART RX interrupt HAL_UART_Receive_DMA(&huart6, (uint8_t*) &proto_buffer_rx, velocity_cmd_length); @@ -299,17 +302,30 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { left_dutycycle += cross_dutycycle; right_dutycycle -= cross_dutycycle; - - wheels_msg.left_vel = left_velocity; - wheels_msg.right_vel = right_velocity; - } //TIMER 2Hz Transmit if (htim->Instance == TIM6) { //TODO odometry - HAL_UART_Transmit_IT(&huart6, tx_buffer, 8); + + pb_ostream_t stream = pb_ostream_from_buffer(proto_buffer_tx, sizeof(proto_buffer_tx)); + + odom.FromWheelVelToOdom(left_velocity, right_velocity); + + status_msg.linear_velocity = odom.GetLinearVelocity(); + status_msg.angular_velocity = odom.GetAngularVelocity(); + + float current_tx_millis = HAL_GetTick(); + status_msg.delta_millis = current_tx_millis - previous_tx_millis; + previous_tx_millis = current_tx_millis; + + status_msg.status = StatusMessage_Status_RUNNING; + + 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); } } @@ -323,14 +339,13 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) { 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); if (status) { linear_velocity = vel_cmd.linear_velocity; angular_velocity = vel_cmd.angular_velocity; - odom.UpdateValues(linear_velocity, angular_velocity); + odom.FromCmdVelToSetpoint(linear_velocity, angular_velocity); float left_setpoint = odom.GetLeftVelocity(); float right_setpoint = odom.GetRightVelocity(); 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 36bf463..21ac630 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 @@ -10,7 +10,8 @@ ser = serial.Serial( parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS, - rtscts=False) + rtscts=False, + exclusive=None) def callback(data): linear = -data.linear.x @@ -23,10 +24,7 @@ def callback(data): ser.write(out_buffer) ser.reset_output_buffer() time.sleep(0.001) - #ser.flush() - - def listener(): while(ser.is_open == False): try: diff --git a/utils/catkin_ws/src/joypad_bridge/scripts/proto_transmit.py b/utils/catkin_ws/src/joypad_bridge/scripts/proto_transmit.py deleted file mode 100644 index 7d1ce00..0000000 --- a/utils/catkin_ws/src/joypad_bridge/scripts/proto_transmit.py +++ /dev/null @@ -1,36 +0,0 @@ -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/serial_receiver.py b/utils/catkin_ws/src/joypad_bridge/scripts/serial_receiver.py new file mode 100755 index 0000000..2b258eb --- /dev/null +++ b/utils/catkin_ws/src/joypad_bridge/scripts/serial_receiver.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python + +import rospy, serial, struct, time +import otto_communication_pb2 +from nav_msgs.msg import Odometry +from serial import SerialException +from google.protobuf.message import DecodeError + + +ser = serial.Serial( + baudrate=115200, + timeout = 1000, + parity=serial.PARITY_NONE, + stopbits=serial.STOPBITS_ONE, + bytesize=serial.EIGHTBITS, + rtscts=False, + exclusive=False) + +def serial_receiver(): + while(ser.is_open == False): + try: + ser.port = '/dev/ttyUSB0' + ser.open() + except SerialException: + print('couldnt open /dev/ttyUSB0') + time.sleep(2) + + print('ttyUSB0 opened') + rospy.init_node('serial_receiver', anonymous=True) + + rospy.Publisher('/odom', Odometry, queue_size=10) + + otto_status = otto_communication_pb2.StatusMessage() + otto_status.linear_velocity = 0 + otto_status.angular_velocity = 0 + otto_status.delta_millis = 0 + otto_status.status = otto_communication_pb2.StatusMessage.Status.UNKNOWN + + encoded_buffer = otto_status.SerializeToString() + + status_length = len(encoded_buffer) + + while (1): + encoded_buffer = ser.read(status_length) + ser.reset_input_buffer() + try: + otto_status.ParseFromString(encoded_buffer) + print(otto_status) + except DecodeError: + print("Decode Error") + + +if __name__ == '__main__': + serial_receiver() diff --git a/utils/py_serial_examples/proto_transmit.py b/utils/py_serial_examples/proto_transmit.py index 2ca3560..ba86410 100644 --- a/utils/py_serial_examples/proto_transmit.py +++ b/utils/py_serial_examples/proto_transmit.py @@ -19,18 +19,18 @@ while (ser.is_open == False): 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() +otto = otto_communication_pb2.StatusMessage() +otto.linear_velocity = 0.3 +otto.angular_velocity = 0.5 +otto.delta_millis = 1000 +otto.status = otto_communication_pb2.StatusMessage.Status.RUNNING + +encode_buffer = otto.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() + print(encode_buffer) time.sleep(0.005) -- 2.52.0