roughly update serial driver and hero vehicle

Author: tz61

decision/hero_vehicle/include/hero_vehicle/dbus_interpreter.h

@@ -26,13 +26,13 @@ using behavior_interface::msg::Chassis;
 class DbusInterpreter
 {
 public:
-    DbusInterpreter(double max_vel, double max_omega, double aim_sens, double deadzone);
+    DbusInterpreter(double max_vel, double max_omega, double aim_sens, double deadzone, double video_link_blank_time);
 
     ~DbusInterpreter();
 
-    void input(const DbusControl::SharedPtr msg);
+    void input_dbus(const DbusControl::SharedPtr msg);
 
-    void input_key(const KeyMouse::SharedPtr msg);
+    void input_video_link(const KeyMouse::SharedPtr msg);
 
     Move::SharedPtr get_move() const;
     geometry_msgs::msg::Twist get_move_ros2_control() const;
@@ -47,11 +47,19 @@ class DbusInterpreter
 
 private:
     bool active;
+    bool keyboard_active_;
+    // video link
+    double kbd_move_x = 0.0, kbd_move_y = 0.0;
+    rclcpp::Time last_video_link_recv_time;
 
+    // dbus (DR16)
     double ls_x, ls_y, rs_x, rs_y, wheel;
     std::string lsw, rsw;
     double mouse_x_ = 0.0, mouse_y_ = 0.0, mouse_z_ = 0.0;
     bool left_button_ = false, right_button_ = false;
+
+    // shared by dbus keyboard and video link keyboard.
+    // TODO (if both active, fall back to video link)
     bool w_ = false, a_ = false, s_ = false;
     bool d_ = false, shift_ = false, ctrl_ = false;
     bool q_ = false, e_ = false, r_ = false;
@@ -61,7 +69,7 @@ class DbusInterpreter
 
     rclcpp::Time last_update_time_;
 
-    double max_vel, max_omega, max_feed, max_shoot, aim_sens, deadzone;
+    double max_vel, max_omega, max_feed, max_shoot, aim_sens, deadzone, video_link_blank_time;
 
     std::thread update_thread;
 

decision/hero_vehicle/src/dbus_interpreter.cpp

@@ -1,11 +1,12 @@
 #include "hero_vehicle/dbus_interpreter.h"
 #include <cmath>
 
-DbusInterpreter::DbusInterpreter(double max_vel, double max_omega, double aim_sens, double deadzone)
-    : max_vel(max_vel), max_omega(max_omega), aim_sens(aim_sens), deadzone(deadzone)
+DbusInterpreter::DbusInterpreter(double max_vel, double max_omega, double aim_sens, double deadzone, double video_link_blank_time)
+    : max_vel(max_vel), max_omega(max_omega), aim_sens(aim_sens), deadzone(deadzone), video_link_blank_time(video_link_blank_time)
 {
     // initialize buttons and axes
     active = false;
+    keyboard_active_ = false;
     ls_x = ls_y = rs_x = rs_y = wheel = 0;
     lsw = rsw = "";
 
@@ -37,7 +38,7 @@ DbusInterpreter::~DbusInterpreter()
     if (update_thread.joinable()) update_thread.join();
 }
 
-void DbusInterpreter::input(const operation_interface::msg::DbusControl::SharedPtr msg)
+void DbusInterpreter::input_dbus(const operation_interface::msg::DbusControl::SharedPtr msg)
 {
     ls_x = msg->ls_x; apply_deadzone(ls_x); // forward is positive
     ls_y = msg->ls_y; apply_deadzone(ls_y); // left is positive
@@ -48,8 +49,9 @@ void DbusInterpreter::input(const operation_interface::msg::DbusControl::SharedP
     rsw = msg->rsw;
 }
 
-void DbusInterpreter::input_key(const operation_interface::msg::KeyMouse::SharedPtr msg)
+void DbusInterpreter::input_video_link(const operation_interface::msg::KeyMouse::SharedPtr msg)
 {
+    keyboard_active_ = msg->active;
     w_ = msg->w;
     a_ = msg->a;
     s_ = msg->s;
@@ -70,13 +72,21 @@ void DbusInterpreter::input_key(const operation_interface::msg::KeyMouse::Shared
     right_button_ = msg->right_button;
     mouse_x_ = msg->mouse_x;
     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();
 }
 
 void DbusInterpreter::update()
 {
-    active = (lsw == "MID");
+    active = (lsw == "MID") || keyboard_active_;
     if (!active)
     {
+        move_->vel_x = 0.0;
+        move_->vel_y = 0.0;
+        move_->omega = 0.0;
+        // printf("Update is not active\n");
+        // TODO: aim ...
         return; // do not update if not active, this prevents yaw and pitch from accumulating in standby
     }
 
@@ -112,22 +122,30 @@ void DbusInterpreter::update()
 
 
     // TODO: Implement Keyboard Actions
-    int move_x = 0, move_y = 0;
-    if(w_) move_x += max_vel;
-    if(s_) move_x -= max_vel;
-    move_->vel_x += move_x;
-
-    if(a_) move_y += max_vel;
-    if(d_) move_y -= max_vel;
-    move_->vel_y += move_y;
-
-    aim_->yaw -= mouse_x_ * aim_sens * PERIOD / 200;   
-    aim_->pitch -= mouse_y_ * aim_sens * PERIOD / 200;  curb(aim_->pitch, M_PI_4);
-    if(q_) aim_->yaw += aim_sens * 0.5 * PERIOD / 1000;
-    if(e_) aim_->yaw -= aim_sens * 0.5 * PERIOD / 1000;
-
-    // To ensure that the change take place only once per key press
     auto current_time = rclcpp::Clock().now();
+    if (current_time.seconds() - last_video_link_recv_time.seconds() > video_link_blank_time) {
+        // Video link timeout
+        keyboard_active_ = false;
+        kbd_move_x = 0.0;
+        kbd_move_y = 0.0;
+        // printf("Inactive video link\n");
+    } else {
+        if(w_) kbd_move_x += deadzone*0.1;
+        if(s_) kbd_move_x -= deadzone*0.1;
+        apply_deadzone(kbd_move_x);
+        move_->vel_x += kbd_move_x*max_vel;
+
+        if(a_) kbd_move_y += deadzone*0.1;
+        if(d_) kbd_move_y -= deadzone*0.1;
+        apply_deadzone(kbd_move_y);
+        move_->vel_y += kbd_move_y*max_vel;
+
+        aim_->yaw -= mouse_x_ * aim_sens * PERIOD / 200;   
+        aim_->pitch -= mouse_y_ * aim_sens * PERIOD / 200;  curb(aim_->pitch, M_PI_4);
+        if(q_) aim_->yaw += aim_sens * 0.5 * PERIOD / 1000;
+        if(e_) aim_->yaw -= aim_sens * 0.5 * PERIOD / 1000;
+    }
+    // To ensure that the change take place only once per key press
 
     // if(current_time.seconds()-last_update_time_.seconds() > 0.2){
     //     if(c_ && !last_c_)  // TOGGLE CHASSIS MODE
@@ -159,9 +177,10 @@ void DbusInterpreter::update()
 
 void DbusInterpreter::apply_deadzone(double &val)
 {
-    if (val < deadzone && val > -deadzone)
-    {
-        val = 0;
+    if (val > deadzone ){
+        val = deadzone;
+    } else if (val < -deadzone){
+        val = -deadzone;
     }
 }
 

decision/hero_vehicle/src/hero_vehicle.cpp

@@ -15,6 +15,7 @@ class HeroVehicle : public rclcpp::Node
         double max_omega = this->declare_parameter("control.rot_vel", 3.0);
         double aim_sens = this->declare_parameter("control.stick_sens", 1.57);
         double deadzone = this->declare_parameter("control.deadzone", 0.05);
+        double video_link_blank_time = this->declare_parameter("control.video_link_blank_time", 0.1);
         std::string aim_topic = this->declare_parameter("aim_topic", "aim");
         std::string shoot_topic = this->declare_parameter("shoot_topic", "shoot");
         std::string chassis_topic = this->declare_parameter("chassis_topic","chassis_cmd");
@@ -23,7 +24,7 @@ class HeroVehicle : public rclcpp::Node
         enable_key_mouse_ = this->declare_parameter("enable_key_mouse_", true);
         enable_ros2_control_ = this->declare_parameter("enable_ros2_control", false);
 
-        interpreter_ = std::make_unique<DbusInterpreter>(max_vel, max_omega, aim_sens, deadzone);
+        interpreter_ = std::make_unique<DbusInterpreter>(max_vel, max_omega, aim_sens, deadzone, video_link_blank_time);
 
         // pub and sub
         if (enable_ros2_control_) {
@@ -40,7 +41,7 @@ class HeroVehicle : public rclcpp::Node
             std::bind(&HeroVehicle::dbus_callback, this, std::placeholders::_1));
         if(enable_key_mouse_){
             key_sub_ = this->create_subscription<operation_interface::msg::KeyMouse>(
-                "key_mouse", 10,
+                "video_link_key_mouse", 10,
                 std::bind(&HeroVehicle::key_callback, this, std::placeholders::_1));
         }
 
@@ -68,12 +69,12 @@ class HeroVehicle : public rclcpp::Node
 
     void dbus_callback(const operation_interface::msg::DbusControl::SharedPtr msg)
     {
-        interpreter_->input(msg);
+        interpreter_->input_dbus(msg);
     }
 
     void key_callback(const operation_interface::msg::KeyMouse::SharedPtr msg)
     {
-        interpreter_->input_key(msg);
+        interpreter_->input_video_link(msg);
     }
 
     void timer_callback()

meta_bringup/config/hero.yaml

@@ -119,8 +119,16 @@ hero_vehicle:
       stick_sens: 5.00 # rad/s
       mouse_sens: 10.0
       deadzone: 0.15 # percentage
+      video_link_blank_time: 0.05 # second
 
 capacitor:
   ros__parameters:
     capacitor_device: "pacific_spirit"
     can_interface: "can_1"
+referee_serial:
+  ros__parameters:
+    regular_link_port: "/dev/tty_REF"
+    regular_link_baud: 115200
+    video_link_port: "/dev/tty_VT02"
+    video_link_baud: 115200
+    isDebug: false

meta_bringup/launch/hero.launch.py

@@ -103,7 +103,14 @@ def generate_launch_description():
         load_controller('omni_chassis_controller'),
         load_controller('shoot_controller')
     ]
-
+    referee_system_node = Node(
+        package='referee_serial',
+        executable='referee_serial_node',
+        name='referee_serial',
+        parameters=[robot_config],
+        output='both',
+        emulate_tty=True,
+    )
     dbus_control_node = Node(
         package='dbus_control',
         executable='dbus_control_node',
@@ -152,9 +159,10 @@ def generate_launch_description():
         load_joint_state_broadcaster,
         # Load controllers
         *register_sequential_loading(load_joint_state_broadcaster, *load_controllers),
-        dbus_control_node,
+        # dbus_control_node,
+        referee_system_node,
         # auto_sentry_node,
         hero_vehicle_node,
-        ahrs_launch,
+        # ahrs_launch,
         super_capacitor_node,
     ])

perception/referee_serial/include/referee_serial/referee_serial.hpp

@@ -60,13 +60,15 @@ class RefereeSerial
      * Interprets the received data and publishes it to the appropriate topic.
      * @note This would call the handle_frame method, and is called by the receive_thread.
      */
-    void receive();
+    void regular_link_receive();
+    void video_link_receive();
 
     /**
      * @brief Method to reopen the serial port.
      * Called when the serial port is closed.
      */
-    void reopen_port();
+    void regular_link_reopen_port();
+    void video_link_reopen_port();
 
     /**
      * @brief Method to handle a frame of data.
@@ -77,7 +79,11 @@ class RefereeSerial
      * @param frame_type The type of the frame.
      */
     template<typename MSG, typename PARSE>
-    void handle_frame(const std::vector<uint8_t>& prefix,
+    void regular_link_handle_frame(const std::vector<uint8_t>& prefix,
+        typename rclcpp::Publisher<MSG>::SharedPtr pub,
+        const std::string frame_type);
+    template<typename MSG, typename PARSE>
+    void video_link_handle_frame(const std::vector<uint8_t>& prefix,
         typename rclcpp::Publisher<MSG>::SharedPtr pub,
         const std::string frame_type);
 
@@ -89,17 +95,20 @@ class RefereeSerial
     rclcpp::Publisher<operation_interface::msg::RobotState>::SharedPtr robot_state_pub_;
 
     // Serial port
-    std::unique_ptr<IoContext> ctx_;
-    std::unique_ptr<SerialPortConfig> config_;
-    std::unique_ptr<SerialDriver> serial_driver_;
-    static std::string dev_name; ///< The path to the serial port.
-    static constexpr const char* dev_null = "/dev/null";
-    static constexpr uint32_t baud = 115200;
+    std::unique_ptr<SerialDriver> regular_link_serial_driver_;
+    std::unique_ptr<SerialDriver> video_link_serial_driver_;
+    
+    std::unique_ptr<IoContext> regular_link_ctx_;
+    std::unique_ptr<IoContext> video_link_ctx_;
+
     static constexpr FlowControl fc = FlowControl::NONE;
     static constexpr Parity pt = Parity::NONE;
     static constexpr StopBits sb = StopBits::ONE;
 
-    std::thread receive_thread; ///< Thread to receive data from the serial port.
+    std::thread regular_link_receive_thread;
+    std::thread video_link_receive_thread;
+
+    bool isDebug = false;
 };
 
 #endif // REFEREE_SERIAL_HPP