update README.md, add initial support for VT03

Author: tz61

README.md

@@ -63,3 +63,17 @@ Simply run
 ```Bash
 colcon build --symlink-install --cmake-args '-DCMAKE_EXPORT_COMPILE_COMMANDS=On'
 ```
+
+## Deploy
+Remove brltty first
+```Bash
+sudo apt purge brltty
+```
+Then install udev rules
+```Bash
+./Meta-ROS/tools/install.sh
+```
+Then plugin the peripheral board, and press
+```
+./Meta-ROS/tools/setupcan.sh
+```

decision/hero_vehicle/include/hero_vehicle/dbus_interpreter.h

@@ -7,6 +7,7 @@
 
 #include "operation_interface/msg/dbus_control.hpp"
 #include "operation_interface/msg/key_mouse.hpp"
+#include "operation_interface/msg/vt03.hpp"
 #include "behavior_interface/msg/move.hpp"
 #include "behavior_interface/msg/shoot.hpp"
 #include "behavior_interface/msg/aim.hpp"
@@ -18,6 +19,7 @@
 
 using operation_interface::msg::DbusControl;
 using operation_interface::msg::KeyMouse;
+using operation_interface::msg::VT03;
 using behavior_interface::msg::Move;
 using behavior_interface::msg::Shoot;
 using behavior_interface::msg::Aim;
@@ -33,6 +35,8 @@ class DbusInterpreter
     void input_dbus(const DbusControl::SharedPtr msg);
 
     void input_video_link(const KeyMouse::SharedPtr msg);
+    
+    void input_video_link_vt03(const VT03::SharedPtr msg);
 
     Move::SharedPtr get_move() const;
     geometry_msgs::msg::Twist get_move_ros2_control() const;

decision/hero_vehicle/src/dbus_interpreter.cpp

@@ -76,6 +76,45 @@ void DbusInterpreter::input_video_link(const operation_interface::msg::KeyMouse:
     if(w_ || s_ || a_ || d_)
         last_video_link_recv_time = rclcpp::Clock().now();
 }
+void DbusInterpreter::input_video_link_vt03(const operation_interface::msg::VT03::SharedPtr msg)
+{
+    // TODO remove keyboard_active_ since it is always true(from generator)
+    keyboard_active_ = true;
+
+    w_ = msg->w;
+    a_ = msg->a;
+    s_ = msg->s;
+    d_ = msg->d;
+    shift_ = msg->shift;
+    ctrl_ = msg->ctrl;
+    q_ = msg->q;
+    e_ = msg->e;
+    r_ = msg->r;
+    f_ = msg->f;
+    g_ = msg->g;
+    z_ = msg->z;
+    x_ = msg->x;
+    c_ = msg->c;
+    v_ = msg->v;
+    b_ = msg->b;
+    left_button_ = msg->mouse_left;
+    right_button_ = msg->mouse_right;
+    // mouse_x_ = msg->mouse_x; TODO not convert to float yet
+    // mouse_y_ = msg->mouse_y;
+    // Video link will send packets even if no keys are pressed, except option panel(p) is active
+    if(w_ || s_ || a_ || d_)
+        last_video_link_recv_time = rclcpp::Clock().now();
+
+    // remote control
+    ls_x = (msg->ch3-1024)/684.0; apply_deadzone(ls_x); // forward is positive
+    ls_y = (msg->ch2-1024)/684.0; apply_deadzone(ls_y); // left is positive
+    rs_x = (msg->ch0-1024)/684.0; apply_deadzone(rs_x); // up is positive
+    rs_y = (msg->ch1-1024)/684.0; apply_deadzone(rs_y); // left is positive
+    wheel = (msg->wheel-1024)/684.0; apply_deadzone(wheel);
+
+    if(msg->cns == 1) lsw = "MID";
+    if(msg->cns == 0) lsw = ""; //rough shit
+}
 
 void DbusInterpreter::update()
 {

decision/hero_vehicle/src/hero_vehicle.cpp

@@ -43,6 +43,9 @@ class HeroVehicle : public rclcpp::Node
             key_sub_ = this->create_subscription<operation_interface::msg::KeyMouse>(
                 "video_link_key_mouse", 10,
                 std::bind(&HeroVehicle::key_callback, this, std::placeholders::_1));
+            vt03_sub_ = this->create_subscription<operation_interface::msg::VT03>(
+                "vt03_msg", 10,
+                std::bind(&HeroVehicle::vt03_callback, this, std::placeholders::_1));
         }
 
         // timer
@@ -57,6 +60,8 @@ class HeroVehicle : public rclcpp::Node
 private:
     rclcpp::Subscription<operation_interface::msg::DbusControl>::SharedPtr dbus_sub_;
     rclcpp::Subscription<operation_interface::msg::KeyMouse>::SharedPtr key_sub_;
+    rclcpp::Subscription<operation_interface::msg::VT03>::SharedPtr vt03_sub_;
+    
     std::unique_ptr<DbusInterpreter> interpreter_;
     rclcpp::Publisher<behavior_interface::msg::Move>::SharedPtr move_pub_;
     rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr move_pub_ros2_control_;
@@ -76,6 +81,10 @@ class HeroVehicle : public rclcpp::Node
     {
         interpreter_->input_video_link(msg);
     }
+    void vt03_callback(const operation_interface::msg::VT03::SharedPtr msg)
+    {
+        interpreter_->input_video_link_vt03(msg);
+    }
 
     void timer_callback()
     {

interfaces/operation_interface/CMakeLists.txt

@@ -20,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/WflyControl.msg"
   "msg/CustomController.msg"
   "msg/RobotState.msg"
+  "msg/VT03.msg"
 )
 ament_export_dependencies(rosidl_default_runtime)
 

interfaces/operation_interface/msg/VT03.msg

@@ -0,0 +1,33 @@
+uint16 ch0
+uint16 ch1
+uint16 ch2
+uint16 ch3
+uint8 cns
+uint8 pause
+bool left_btn 
+bool right_btn
+uint16 wheel
+bool trigger
+uint16 mouse_x
+uint16 mouse_y
+uint16 mouse_z
+uint8 mouse_left
+uint8 mouse_right
+uint8 mouse_middle
+
+bool w
+bool s
+bool a
+bool d
+bool shift
+bool ctrl
+bool q
+bool e
+bool r
+bool f
+bool g
+bool z
+bool x
+bool c
+bool v
+bool b

meta_bringup/config/hero.yaml

@@ -130,6 +130,7 @@ referee_serial:
     regular_link_port: "/dev/tty_REF"
     regular_link_baud: 115200
     video_link_port: "/dev/tty_VT02"
-    video_link_baud: 115200
+    video_link_baud: 921600
     isDebug: false
-    debugUnhandledShown: false
+    debugUnhandledShown: false
+    warningShown: false

perception/referee_serial/CMakeLists.txt

@@ -31,6 +31,7 @@ add_library(referee_serial SHARED
   src/power_state.cpp
   src/custom_controller.cpp
   src/robot_state.cpp
+  src/vt03_body.cpp
   src/crc.cpp)
 
 target_include_directories(referee_serial PRIVATE

perception/referee_serial/include/referee_serial/referee_serial.hpp

@@ -16,9 +16,11 @@
 #include "operation_interface/msg/power_state.hpp"
 #include "operation_interface/msg/custom_controller.hpp"
 #include "operation_interface/msg/robot_state.hpp"
+#include "operation_interface/msg/vt03.hpp"
 
 #define REFEREE_SOF 0xA5
-#define VIDEO_LINK_REMOTECONTROL_SOF 0xA9
+#define VIDEO_LINK_REMOTECONTROL_LOWER_SOF 0xA9
+#define VIDEO_LINK_REMOTECONTROL_UPPER_SOF 0x53
 
 using drivers::serial_driver::FlowControl;
 using drivers::serial_driver::Parity;
@@ -38,10 +40,12 @@ class RefereeSerial
     enum rx_status_t {
         WAIT_STARTING_BYTE,  // receive bytes one by one, waiting for 0xA5
         WAIT_REMAINING_HEADER,  // receive remaining header after SOF
-        WAIT_CMD_ID_DATA_TAIL  // receive cmd_id, data section and 2-byte CRC16 tailing
+        WAIT_CMD_ID_DATA_TAIL,  // receive cmd_id, data section and 2-byte CRC16 tailing
+        VT03_REMAINING
     };
     enum {
-        FRAME_HEADER_LENGTH=5
+        FRAME_HEADER_LENGTH=5,
+        FRAME_VT03_LENGTH=21
     };
 /**
      * @brief Constructor for the RefereeSerial class.
@@ -104,6 +108,7 @@ class RefereeSerial
     rclcpp::Publisher<operation_interface::msg::PowerState>::SharedPtr power_state_pub_;
     rclcpp::Publisher<operation_interface::msg::CustomController>::SharedPtr custom_controller_pub_;
     rclcpp::Publisher<operation_interface::msg::RobotState>::SharedPtr robot_state_pub_;
+    rclcpp::Publisher<operation_interface::msg::VT03>::SharedPtr vt03_msg_pub_;
 
     // Serial port
     std::unique_ptr<SerialDriver> regular_link_serial_driver_;
@@ -123,6 +128,7 @@ class RefereeSerial
 
     bool isDebug = false;
     bool debugUnhandledShown = false;
+    bool warningShown = false;
 };
 
 #endif // REFEREE_SERIAL_HPP

perception/referee_serial/include/referee_serial/vt03_body.hpp

@@ -0,0 +1,41 @@
+#ifndef VT03_BODY_HPP
+#define VT03_BODY_HPP
+#include "operation_interface/msg/vt03.hpp"
+class VT03_MSG
+{
+public:
+    struct __attribute__((packed)) FrameType
+    {
+        uint16_t frame_header;
+
+        uint16_t ch0:11, ch1:11, ch2:11, ch3:11;
+        uint8_t cns:2, pause:1, left_btn:1, right_btn:1;
+        uint16_t wheel:11;
+        uint8_t trigger:1, dummy_1:3;
+
+        int16_t mouse_x, mouse_y, mouse_z;
+        uint8_t mouse_left:2, mouse_right:2, mouse_middle:2, dummy_2:2;
+
+        uint8_t w:1, s:1, a:1, d:1, shift:1, ctrl:1, q:1, e:1, r:1, f:1, g:1, z:1, x:1, c:1, v:1, b:1;
+
+        uint16_t crc16;
+
+    };
+
+    FrameType interpreted; /**< The interpreted frame */ 
+
+    /**
+     * @brief Constructs a KeyMouse object from a frame.
+     * This would interpret the frame by copying the data into the FrameType struct.
+     * @param frame The frame to construct the KeyMouse object from.
+     */
+    VT03_MSG(const std::vector<uint8_t> &frame);
+
+    /**
+     * @brief Sets the message based on the current state.
+     * This function copies the current state data into the message object.
+     * @note Should be called before publishing the message.
+     */
+    operation_interface::msg::VT03 msg();
+};
+#endif