receive cmd_vel and send velocity with protobuf

diff --git a/otto_controller/Core/Inc/odometry.h b/otto_controller/Core/Inc/odometry.h
index 85f24f7c93a9b246b8db644487951b5ab965acc9..417f062d02b1043fa090421368b25a4e2a4d06ec 100644 (file)
--- 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 d19ac9fa22b303ba3f0a744960765794b5394492..485ec55ad49322898cedccc63ae472ac3e32066d 100644 (file)
--- 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;\r
 size_t status_msg_length;\r
 bool tx_status;\r
+float previous_tx_millis;\r
 \r
 /* USER CODE END PV */\r
 \r
@@ -203,9 +204,11 @@ int main(void) {
                                          sizeof(proto_buffer_tx));\r
 \r
   pb_encode(&out_pb_stream, VelocityCommand_fields, &vel_cmd);\r
-\r
   velocity_cmd_length = out_pb_stream.bytes_written;\r
 \r
+  pb_encode(&out_pb_stream, StatusMessage_fields, &status_msg);\r
+  status_msg_length = out_pb_stream.bytes_written;\r
+\r
   //Enables UART RX interrupt\r
   HAL_UART_Receive_DMA(&huart6, (uint8_t*) &proto_buffer_rx,\r
                        velocity_cmd_length);\r
@@ -299,17 +302,30 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
 \r
     left_dutycycle += cross_dutycycle;\r
     right_dutycycle -= cross_dutycycle;\r
-\r
-    wheels_msg.left_vel = left_velocity;\r
-    wheels_msg.right_vel = right_velocity;\r
-\r
   }\r
 \r
   //TIMER 2Hz Transmit\r
   if (htim->Instance == TIM6) {\r
 \r
     //TODO odometry\r
-    HAL_UART_Transmit_IT(&huart6, tx_buffer, 8);\r
+\r
+    pb_ostream_t stream = pb_ostream_from_buffer(proto_buffer_tx, sizeof(proto_buffer_tx));\r
+\r
+    odom.FromWheelVelToOdom(left_velocity, right_velocity);\r
+\r
+    status_msg.linear_velocity = odom.GetLinearVelocity();\r
+    status_msg.angular_velocity = odom.GetAngularVelocity();\r
+\r
+    float current_tx_millis = HAL_GetTick();\r
+    status_msg.delta_millis = current_tx_millis - previous_tx_millis;\r
+    previous_tx_millis = current_tx_millis;\r
+\r
+    status_msg.status = StatusMessage_Status_RUNNING;\r
+\r
+    pb_encode(&stream, StatusMessage_fields, &status_msg);\r
+\r
+    HAL_UART_Transmit_DMA(&huart6,(uint8_t*) &proto_buffer_tx, status_msg_length);\r
+//    HAL_UART_Transmit(&huart6,(uint8_t*) &proto_buffer_tx, status_msg_length, 100);\r
   }\r
 }\r
 \r
@@ -323,14 +339,13 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {
   pb_istream_t stream = pb_istream_from_buffer(proto_buffer_rx,\r
                                                velocity_cmd_length);\r
 \r
-  /* Now we are ready to decode the message. */\r
   bool status = pb_decode(&stream, VelocityCommand_fields, &vel_cmd);\r
 \r
   if (status) {\r
     linear_velocity = vel_cmd.linear_velocity;\r
     angular_velocity = vel_cmd.angular_velocity;\r
 \r
-    odom.UpdateValues(linear_velocity, angular_velocity);\r
+    odom.FromCmdVelToSetpoint(linear_velocity, angular_velocity);\r
 \r
     float left_setpoint = odom.GetLeftVelocity();\r
     float right_setpoint = odom.GetRightVelocity();\r

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 36bf46328137ee95c290e950795e6cec94dd2f8b..21ac63002f6c79f89e9cde8d26c5f40ef8bfa85d 100755 (executable)
--- 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 (file)
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 (executable)
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 2ca35609ff43a314bd05bc9e335ddb73bfd43d74..ba86410a6161a0cdbdf09cd906f2e4c0509093ca 100644 (file)
--- 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)