feat(trajectory): Implement dynamic trajectory control in CustomRobot

- Added methods for trajectory control: trajControl, startTraj, and stopTraj.
- Integrated trajectory control into command processing for starting and stopping dynamic trajectories.
- Introduced a recurrent thread for continuous trajectory updates at a frequency of 50Hz.
- Initialized trajectory control state management with atomic flags.

include/custom_robot.hpp

@@ -15,6 +15,7 @@
 #include <vector>
 #include <nlohmann/json.hpp>
 #include <unitree/robot/go2/robot_state/robot_state_client.hpp>
+#include <unitree/common/thread/recurrent_thread.hpp>
 
 namespace custom {
 
@@ -47,6 +48,10 @@ public:
     bool processRechargeCmd(const std::string& cmd, const nlohmann::json& message);
     void printServiceList(const std::vector<unitree::robot::go2::ServiceState>& serviceList, int filterStatus = -1);
 
+    void trajControl();
+    bool startTraj();
+    void stopTraj();
+
 private:
     std::string generateRandomClientId() const;
     std::unique_ptr<Controller> controller_;
@@ -59,6 +64,8 @@ private:
     CustomConfig config_;  
     std::atomic<bool> running_;
     std::atomic<bool> initialized_;
+    unitree::common::ThreadPtr traj_th_;
+    std::atomic<bool> traj_running_;
 };
 
 } // namespace custom