Merge pull request #2 from ZhuoyueLi/main

FrankaProxy framework done the message dealing and test for state publish and control request

Author: xinkai-jiang

CMakeLists.txt

@@ -1,24 +1,49 @@
 cmake_minimum_required(VERSION 3.10)
-project(FRANKA_CONTROL_PROXY)
+project(Franka_Control_Proxy)
 
-# Set C++ standard
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED True)
 
-find_package(Franka REQUIRED)
+#include
+include_directories(
+    include
+    include/control_mode
+    include/protocol
+    include/debugger
+)
 
-# Find libzmq package
+# dependencies
+find_package(Franka REQUIRED)
+find_package(Threads REQUIRED)
 find_package(PkgConfig REQUIRED)
 pkg_check_modules(ZMQ REQUIRED libzmq)
 find_package(yaml-cpp REQUIRED)
 
+
+# collect source
+file(GLOB_RECURSE SOURCES
+    src/*.cpp
+)
+
 # Include directories
 include_directories(include ${YAML_CPP_INCLUDE_DIR} ${Franka_INCLUDE_DIRS})
 
-# Add executable target
-add_executable(leader src/human_controller.cpp)
-add_executable(follower src/follow.cpp)
+# build proxy
+add_executable(proxy ${SOURCES})
+
+# connect with dependencies
+target_link_libraries(proxy
+    ${Franka_LIBRARIES}
+    ${ZMQ_LIBRARIES}
+    ${YAML_CPP_LIBRARIES}
+    Threads::Threads
+)
+
+#include path
+target_include_directories(proxy PRIVATE
+    ${franka_INCLUDE_DIRS}
+    ${ZMQ_INCLUDE_DIRS}
+)
+
+
 
-# Link libraries to your target (franka, yaml-cpp, zmq)
-target_link_libraries(leader PRIVATE ${YAML_CPP_LIBRARIES} ${Franka_LIBRARIES} ${ZMQ_LIBRARIES})
-target_link_libraries(follower PRIVATE ${YAML_CPP_LIBRARIES} ${Franka_LIBRARIES} ${ZMQ_LIBRARIES})

RobotConfig.yaml

@@ -1,4 +1,4 @@
-RobotIP: 172.16.2.2
-PublisherAddr: tcp://141.3.53.152:5555
-ServiceAddr: tcp://141.3.53.152:5556
+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

include/FrankaProxy.h

@@ -0,0 +1,98 @@
+#ifndef FRANKA_SERVER_HPP
+#define FRANKA_SERVER_HPP
+
+#include <zmq.hpp>
+#include <thread>
+#include <atomic>
+#include <mutex>
+#include <string>
+#include <memory>
+
+#include <franka/robot.h>
+#include <franka/model.h>
+#include <franka/robot_state.h>
+#include <yaml-cpp/yaml.h>
+#include "protocol/franka_arm_state.hpp"
+#include "control_mode/abstract_control_mode.h"
+
+class FrankaProxy {
+
+public:
+    // Constructor & Destructor
+    explicit FrankaProxy(const std::string& config_path);// Constructor that initializes the proxy with a configuration file
+    ~FrankaProxy();// Destructor to clean up resources
+
+    // Core server operations
+    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
+    
+    // State management
+    franka::RobotState getCurrentState();// Return the current state of the robot
+
+    // Mode management
+    void registerControlMode(const std::string& mode, std::unique_ptr<AbstractControlMode> control_mode);//register the map
+    void setControlMode(const std::string& mode);
+    
+    
+    
+    // Configuration
+    void displayConfig() const;
+    
+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?
+
+    // Thread functions
+    void statePublishThread();// ZMQ PUB, Publishes the current state of the robot 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
+
+    
+    // Message handling
+    void handleServiceRequest(const std::string& request, std::string& response);
+    
+    
+private:
+    // Configuration
+    YAML::Node proxy_config_;
+    std::string robot_ip_;
+    std::string state_pub_addr_;
+    std::string service_addr_;
+    
+    // Franka robot
+    std::shared_ptr<franka::Robot> robot_;
+    std::shared_ptr<franka::Model> model_;
+    
+    // ZMQ communication
+    zmq::context_t context_;
+    zmq::socket_t pub_socket_;
+    zmq::socket_t sub_socket_;
+    zmq::socket_t rep_socket_;
+    
+    // Threading
+    std::thread state_pub_thread_;//statePublishThread()
+    std::thread service_thread_;//responseSocketThread()
+    std::thread control_thread_;//controlLoopThread()
+    
+    
+    // Synchronization
+    std::atomic<bool> in_control_;//for threads
+    
+    // State data
+    franka::RobotState current_state_;
+    
+    //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 SOCKET_TIMEOUT_MS = 100;
+    static constexpr int MAX_MESSAGE_SIZE = 4096;
+};
+
+#endif // FRANKA_SERVER_HPP

include/FrankaProxy.hpp

@@ -35,15 +35,18 @@ class RobotConfig {
     }
 
     // Function to get a value as a string
-    std::string getValue(const std::string& key) const {
+
+    std::string getValue(const std::string& key, const std::string& default_value = "") const {
         auto it = config_data.find(key);
         if (it != config_data.end()) {
             return it->second;
         } else {
-            std::cerr << "Warning: Key not found in config: " << key << std::endl;
-            return "";
+        std::cerr << "Warning: Key not found in config: " << key << ", using default: " << default_value << std::endl;
+        return default_value;
         }
-    }
+    }   
+
+    
 
     // Function to display the loaded configuration
     void display() const {