Add the gripper part and follower logic

CMakeLists.txt

@@ -1,6 +1,5 @@
+project(FrankaControlProxy)
 cmake_minimum_required(VERSION 3.10)
-project(Franka_Control_Proxy)
-
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED True)
 

P1_arm.yaml

@@ -0,0 +1,7 @@
+type:Arm
+follower:true
+robot_ip:172.16.1.2
+state_pub_addr:tcp://141.3.53.152:51000
+service_addr:tcp://141.3.53.152:51001
+sub_addr:tcp://141.3.53.152:52000
+useGripper:true

P1_gripper.yaml

@@ -0,0 +1,7 @@
+type:Gripper
+follower:true
+robot_ip:172.16.1.2
+gripper_pub_addr:tcp://141.3.53.152:51003
+service_addr:tcp://141.3.53.152:51004
+sub_addr:tcp://141.3.53.152:52003
+useGripper:true

P2_arm.yaml

@@ -0,0 +1,6 @@
+type:Arm
+follower:false
+robot_ip:172.16.2.2
+state_pub_addr:tcp://141.3.53.152:52000
+service_addr:tcp://141.3.53.152:52001
+useGripper:true

P2_gripper.yaml

@@ -0,0 +1,6 @@
+type:Gripper
+follower:false
+robot_ip:172.16.2.2
+gripper_pub_addr:tcp://141.3.53.152:52003
+service_addr:tcp://141.3.53.152:52004
+useGripper:true

RobotConfig copy.yaml

@@ -0,0 +1,7 @@
+type:Gripper
+follower:true
+robot_ip:172.16.1.2
+gripper_pub_addr:tcp://141.3.53.152:51003
+service_addr:tcp://141.3.53.152:51004
+sub_addr:tcp://141.3.53.152:51005
+useGripper:true

RobotConfig.yaml

@@ -1,4 +1,9 @@
-robot_ip: 172.16.2.2
-state_pub_addr: tcp://141.3.53.152:5555
-service_addr: tcp://141.3.53.152:5556
-useGripper: true
+type:Arm
+follower:false
+robot_ip:172.16.2.2
+state_pub_addr:tcp://141.3.53.152:5555
+service_addr:tcp://141.3.53.152:5556
+gripper_pub_addr:tcp://141.3.53.152:5557
+sub_addr:tcp://141.3.53.152:5558
+gripper_sub_addr:tcp://141.3.53.152:5559
+useGripper:true

include/FrankaProxy.hpp

@@ -11,9 +11,11 @@
 #include <franka/robot.h>
 #include <franka/model.h>
 #include <franka/robot_state.h>
+#include <franka/gripper.h>
 #include <yaml-cpp/yaml.h>
 #include "protocol/franka_arm_state.hpp"
 #include "control_mode/abstract_control_mode.h"
+#include "protocol/franka_gripper_state.hpp"
 
 class FrankaProxy {
 
@@ -26,10 +28,10 @@ class FrankaProxy {
     bool start();// Starts the Franka server, initializing the robot and communication sockets
     void stop();// Stops the server, cleaning up resources and shutting down communication
     void spin();// Main loop for processing requests
-
+    std::string getType() const { return type_; } // Returns the type of the proxy (e.g., "Arm" or "Gripper")
     // State management
     franka::RobotState getCurrentState();// Return the current state of the robot
-
+    protocol::FrankaGripperState getCurrentGripper();// Return the current state of the gripper
     // Mode management
     void registerControlMode(const std::string& mode, std::unique_ptr<AbstractControlMode> control_mode);//register the map
     void setControlMode(const std::string& mode);
@@ -41,56 +43,81 @@ class FrankaProxy {
 
 private:
     // Initialization
-    void initialize(const std::string &filename);// Initializes the FrankaProxy with the given configuration file
-    void setupCommunication();// Sets up ZMQ communication sockets and threads?
-
+    void initialize(const std::string &filename);// Initializes the FrankaProxy with the given configuration file and set up communication sockets
+    //Start
+    bool startArm();// Starts the arm control loop and initializes the necessary threads
+    bool startGripper();// Starts the gripper control loop and initializes the necessary threads
+    //Stop
+    void stopArm();// Stops the arm control loop and cleans up resources
+    void stopGripper();// Stops the gripper control loop and cleans up resources
     // Thread functions
     void statePublishThread();// ZMQ PUB, Publishes the current state of the robot at a fixed rate
+    void gripperPublishThread();// ZMQ PUB, Publishes the current gripper state at a fixed rate
     void responseSocketThread();// ZMQ REP,responds to incoming requests from clients
     void controlLoopThread();// Main control loop for processing commands and updating the robot state
-
-
+    void stateSubscribeThread();// ZMQ SUB, Subscribes to the state updates from a leader robot (for follower mode)
+    void gripperSubscribeThread();// ZMQ SUB, Subscribes to the gripper updates
     // Message handling
-    void handleServiceRequest(const std::string& request, std::string& response);
+    void handleServiceRequest(const std::vector<uint8_t>& request, std::vector<uint8_t>& response) ;
 
 
 private:
     // Configuration
     YAML::Node proxy_config_;
+    std::string type_;
     std::string robot_ip_;
-    std::string state_pub_addr_;
     std::string service_addr_;
-
+    std::string state_pub_addr_;
+    std::string gripper_pub_addr_;
+    std::string state_sub_addr_;
+    std::string gripper_sub_addr_;
+    std::string follower_ = "false";
     // Franka robot
     std::shared_ptr<franka::Robot> robot_;
     std::shared_ptr<franka::Model> model_;
+    //Franka gripper
+    std::shared_ptr<franka::Gripper> gripper_;
 
     // ZMQ communication
     zmq::context_t context_;
-    zmq::socket_t pub_socket_;
-    zmq::socket_t sub_socket_;
+    //zmq::socket_t pub_socket_;
+    zmq::socket_t pub_arm_socket_;
+    zmq::socket_t pub_gripper_socket_;
+    //zmq::socket_t sub_socket_;
+    zmq::socket_t sub_arm_socket_;
+    zmq::socket_t sub_gripper_socket_;
     zmq::socket_t rep_socket_;
 
     // Threading
+    std::thread control_thread_;//controlLoopThread()
+
     std::thread state_pub_thread_;//statePublishThread()
+    std::thread gripper_pub_thread_;//gripperPublishThread()
+
     std::thread service_thread_;//responseSocketThread()
-    std::thread control_thread_;//controlLoopThread()
+
+    std::thread state_sub_thread_;//stateSubscribeThread(),just for follower
+    std::thread gripper_sub_thread_;//gripperSubscribeThread(),just for follower
 
 
     // Synchronization
     std::atomic<bool> in_control_;//for threads
 
     // State data
     franka::RobotState current_state_;
+    franka::RobotState leader_state_; //for follower to generate control
+    // Gripper data
+    franka::GripperState current_gripper_state_;
+    franka::GripperState leader_gripper_state_; //for follower
 
     //Control mode
-    std::string mode_name;
     AbstractControlMode* current_mode_ = nullptr;
     std::mutex control_mutex_;
     std::map<std::string, std::shared_ptr<AbstractControlMode>> control_modes_map_;
 
     // TODO: put all the Constants to a config file
     static constexpr int STATE_PUB_RATE_HZ = 100;
+    static constexpr int GRIPPER_PUB_RATE_HZ = 100;
     static constexpr int SOCKET_TIMEOUT_MS = 100;
     static constexpr int MAX_MESSAGE_SIZE = 4096;
 };

include/RobotConfig.hpp

@@ -46,7 +46,6 @@ class RobotConfig {
         }
     }   
 
-
 
     // Function to display the loaded configuration
     void display() const {

include/control_mode/abstract_control_mode.h

@@ -5,6 +5,9 @@
 #include <franka/model.h>
 #include <franka/robot_state.h>
 #include <franka/exception.h>
+#include <memory>//for std::shared_ptr
+#include <mutex>
+//todo:reform and check the leadter state get and the is_running
 class AbstractControlMode {
 public:
     // Virtual destructor for proper cleanup in derived classes
@@ -13,28 +16,55 @@ class AbstractControlMode {
     //virtual void initialize(const RobotState& initial_state);
     virtual void start() = 0;
     virtual void stop() = 0;
-
+    // Get the mode ID for this control mode
+    virtual int getModeID() const = 0; // Return the mode ID as an integer
     void setRobot(std::shared_ptr<franka::Robot> robot) {
         robot_ = std::move(robot);
     }
     void setModel(std::shared_ptr<franka::Model> model) {
         model_ = std::move(model);
     }
-    virtual franka::RobotState UpdateState() const {
+    // Set the current state of the robot into variable current_state_
+    void setCurrentState(const franka::RobotState& state) {
+        std::lock_guard<std::mutex> lock(state_mutex_);
+        current_state_ = state;
+    }
+
+    // get current state of robot
+    virtual franka::RobotState getRobotState() const {
         std::lock_guard<std::mutex> lock(state_mutex_);
         return current_state_;
     }
+
+    void setLeaderState(const franka::RobotState& leader_state) {
+        std::lock_guard<std::mutex> lock(leader_mutex_);
+        leader_state_ = std::make_shared<franka::RobotState>(leader_state);
+    }
+
+    std::shared_ptr<const franka::RobotState> getLeaderState() const {
+        std::lock_guard<std::mutex> lock(leader_mutex_);
+        return leader_state_;
+    }
+    //for mode use:
+    // auto leader_ptr = getLeaderState();
+    //     
+
+    // const franka::RobotState& leader = *leader_state_ptr;
+    //franka::Torques tau = computeTorqueFromLeader(leader);
+
+
+
 protected:
     // Protected constructor to prevent direct instantiation
     AbstractControlMode() = default;
-
     // Protected setup function for derived classes
     std::shared_ptr<franka::Robot> robot_;
     std::shared_ptr<franka::Model> model_;
     franka::RobotState current_state_;
     mutable std::mutex state_mutex_;
     bool is_running_ = false;
-
+    std::shared_ptr<franka::RobotState> leader_state_;
+    mutable std::mutex leader_mutex_; // Protects access to leader_state_
 };
 
 #endif // ABSTRACT_CONTROL_MODE_H

include/control_mode/idle_control_mode.hpp

@@ -2,9 +2,17 @@
 #define IDLE_CONTROL_MODE_H
 
 #include "abstract_control_mode.h"
-#include <memory>
+#include <franka/robot.h>
+#include <franka/model.h>
+#include <franka/robot_state.h>
+#include <franka/exception.h>
+#include <memory>//for std::shared_ptr
+#include <mutex>
+#include <franka/exception.h>
 #include <iostream>
 
+
+
 class IdleControlMode : public AbstractControlMode {
 public:
     IdleControlMode() = default;
@@ -13,9 +21,8 @@ class IdleControlMode : public AbstractControlMode {
     //void initialize(const franka::RobotState& initial_state) override;
     void start() override;
     void stop() override;
-
-private:
-    void updateOnce();  // 使用 readOnce 读取当前状态
+    int getModeID() const override;
 };
 
+
 #endif // IDLE_CONTROL_MODE_H

include/control_mode/joint_pd_test_mode.hpp

@@ -0,0 +1,19 @@
+#ifndef JOINT_PD_CONTROL_TEST_HPP
+#define JOINT_PD_CONTROL_TEST_HPP
+
+#include "abstract_control_mode.h"
+#include <franka/robot_state.h>
+#include <array>
+#include <functional>
+
+class JointPDControlMode : public AbstractControlMode {
+public:
+    JointPDControlMode();
+    ~JointPDControlMode() override;
+
+    void start() override;
+    void stop() override;
+    int getModeID() const override;
+};
+
+#endif // JOINT_PD_CONTROL_TEST_HPP

include/control_mode/zero_torque_mode.hpp

@@ -0,0 +1,25 @@
+#ifndef ZERO_TORQUE_MODE_HPP
+#define ZERO_TORQUE_MODE_HPP
+
+#include "abstract_control_mode.h"
+#include <franka/robot.h>
+#include <franka/model.h>
+#include <franka/robot_state.h>
+#include <franka/exception.h>
+#include <memory>//for std::shared_ptr
+#include <mutex>
+#include <franka/exception.h>
+#include <iostream>
+
+class ZeroTorqueMode : public AbstractControlMode {
+public:
+    ZeroTorqueMode() = default;
+    ~ZeroTorqueMode() override = default;
+
+    //void initialize(const franka::RobotState& initial_state) override;
+    void start() override;
+    void stop() override;
+    int getModeID() const override;
+};
+
+#endif // ZERO_TORQUE_MODE_HPP