Merge pull request #36 from d-nakamichi/refactoring

Refactoring about khi_robot_control

Author: matsui-hiro

khi_robot_bringup/config/duaro_controllers.yaml

@@ -1,14 +1,16 @@
+  khi_robot_param:
+    robot: WD002N
+    arm:
+      arm1:
+        - duaro_lower_arm_controller
+      arm2:
+        - duaro_upper_arm_controller
+
   # Publish all joint states -----------------------------------
   joint_state_controller:
     type: joint_state_controller/JointStateController
     publish_rate: 50  
 
-  khi_robot_controllers:
-    robot: WD002N
-    names:
-      - duaro_lower_arm_controller
-      - duaro_upper_arm_controller
-
   # Position - Right and Left Joint Position Trajectory Controllers -------------------
   duaro_lower_arm_controller:
     type: "position_controllers/JointTrajectoryController"

khi_robot_bringup/config/rs007l_controllers.yaml

@@ -1,13 +1,14 @@
+  khi_robot_param:
+    robot: RS007L
+    arm:
+      arm1:
+        - rs007l_arm_controller
+
   # Publish all joint states -----------------------------------
   joint_state_controller:
     type: joint_state_controller/JointStateController
     publish_rate: 50  
 
-  khi_robot_controllers:
-    robot: RS007L
-    names:
-      - rs007l_arm_controller
-
   # Position - Joint Position Trajectory Controller -------------------
   rs007l_arm_controller:
     type: "position_controllers/JointTrajectoryController"

khi_robot_bringup/config/rs007n_controllers.yaml

@@ -1,13 +1,14 @@
+  khi_robot_param:
+    robot: RS007N
+    arm:
+      arm1:
+        - rs007n_arm_controller
+
   # Publish all joint states -----------------------------------
   joint_state_controller:
     type: joint_state_controller/JointStateController
     publish_rate: 50  
 
-  khi_robot_controllers:
-    robot: RS007N
-    names:
-      - rs007n_arm_controller
-
   # Position - Joint Position Trajectory Controller -------------------
   rs007n_arm_controller:
     type: "position_controllers/JointTrajectoryController"

khi_robot_bringup/config/rs080n_controllers.yaml

@@ -1,13 +1,15 @@
+
+  khi_robot_param:
+    robot: RS080N
+    arm:
+      arm1:
+        - rs080n_arm_controller
+
   # Publish all joint states -----------------------------------
   joint_state_controller:
     type: joint_state_controller/JointStateController
     publish_rate: 50  
 
-  khi_robot_controllers:
-    robot: RS080N
-    names:
-      - rs080n_arm_controller
-
   # Position - Joint Position Trajectory Controller -------------------
   rs080n_arm_controller:
     type: "position_controllers/JointTrajectoryController"

khi_robot_control/CMakeLists.txt

@@ -54,6 +54,9 @@ endif()
 find_library(krnx_LIBRARIES lib${krnx_lib_name}.so PATHS ${PROJECT_SOURCE_DIR}/lib)
 message(STATUS "Found KRNX library at: ${krnx_LIBRARIES}")
 
+## Specify C++11
+add_definitions(-std=c++11)
+
 ## Declare a C++ library
 add_library(khi_robot_client src/khi_robot_client.cpp src/khi_robot_krnx_driver.cpp)
 

khi_robot_control/include/khi_robot_client.h

@@ -48,27 +48,23 @@ class KhiRobotClient
     KhiRobotClient(){};
     ~KhiRobotClient(){};
 
-    bool open( std::string robot, std::string ip, double period, JointData joint, bool in_simulation = false );
-    bool activate( JointData *joint );
-    bool hold( const JointData joint );
-    void deactivate();
+    bool open( const std::string& ip, const double& period, KhiRobotData& data, const bool in_simulation = false );
+    bool activate( KhiRobotData& data );
+    bool hold( const KhiRobotData& data );
+    void deactivate( const KhiRobotData& data );
     void close();
 
-    void write( const JointData joint );
-    void read( JointData *joint );
+    void write( const KhiRobotData& data );
+    void read( KhiRobotData& data );
 
-    int updateState();
+    int updateState( const KhiRobotData& data );
     int getStateTrigger();
-    bool getPeriodDiff( double *diff );
+    bool getPeriodDiff( double& diff );
     void startCommandService();
 
 private:
     int cont_no;
-    JointData joint_cmd;
-    JointData joint_pos;
     KhiRobotDriver *driver;
-
-    void printJointData( std::string name, const JointData joint );
 };
 
 } // namespace

khi_robot_control/include/khi_robot_driver.h

@@ -41,26 +41,34 @@
 namespace khi_robot_control
 {
 #define KHI_MAX_CONTROLLER 8
+#define KHI_MAX_ARM 2
 #define KHI_MAX_JOINT 18
 #define KHI_MAX_SIG_SIZE 512
 
-struct JointData
+struct KhiRobotArmData
 {
-    int joint_num;
+    int jt_num;
     std::string name[KHI_MAX_JOINT];
+    int type[KHI_MAX_JOINT];
     double cmd[KHI_MAX_JOINT];
     double pos[KHI_MAX_JOINT];
     double vel[KHI_MAX_JOINT];
     double eff[KHI_MAX_JOINT];
+    double home[KHI_MAX_JOINT];
 };
 
-struct RobotInfo
+struct KhiRobotData
+{
+    std::string robot_name;
+    int arm_num;
+    KhiRobotArmData arm[KHI_MAX_ARM];
+};
+
+struct KhiRobotControllerInfo
 {
     int state;
     int state_trigger;
     std::string ip_address;
-    std::string robot_name;
-    int arm_num;
     double period;
 };
 
@@ -119,23 +127,21 @@ class KhiRobotDriver
     {
         for ( int cno = 0; cno < KHI_MAX_CONTROLLER; cno++ )
         {
-            robot_info[cno].state = INIT;
-            robot_info[cno].state_trigger = NONE;
-            robot_info[cno].ip_address = "127.0.0.1";
-            robot_info[cno].robot_name = "";
-            robot_info[cno].arm_num = -1;
+            cont_info[cno].state = INIT;
+            cont_info[cno].state_trigger = NONE;
+            cont_info[cno].ip_address = "127.0.0.1";
         }
 
         driver_name = __func__;
     }
 
-    int getState( const int cont_no )
+    int getState( const int& cont_no )
     {
         if ( ( cont_no < 0 ) || ( cont_no > KHI_MAX_CONTROLLER ) ) { return NOT_REGISTERED; }
-        else                                                       { return robot_info[cont_no].state; }
+        else                                                       { return cont_info[cont_no].state; }
     }
 
-    std::string getStateName( const int cont_no )
+    std::string getStateName( const int& cont_no )
     {
         int state;
         std::string name = "";
@@ -149,7 +155,7 @@ class KhiRobotDriver
         return name;
     }
 
-    bool setState( const int cont_no, const int state )
+    bool setState( const int& cont_no, const int& state )
     {
         if ( !contLimitCheck( cont_no, KHI_MAX_CONTROLLER ) ) { return false; }
 
@@ -159,16 +165,16 @@ class KhiRobotDriver
         }
         else
         {
-            if ( robot_info[cont_no].state != state )
+            if ( cont_info[cont_no].state != state )
             {
-                infoPrint( "State %d: %s -> %s", cont_no, KhiRobotStateName[robot_info[cont_no].state].c_str(), KhiRobotStateName[state].c_str() );
-                robot_info[cont_no].state = state;
+                infoPrint( "State %d: %s -> %s", cont_no, KhiRobotStateName[cont_info[cont_no].state].c_str(), KhiRobotStateName[state].c_str() );
+                cont_info[cont_no].state = state;
             }
             return true;
         }
     }
 
-    bool isTransitionState( const int cont_no )
+    bool isTransitionState( const int& cont_no )
     {
         int state;
 
@@ -183,20 +189,20 @@ class KhiRobotDriver
         }
     }
 
-    int getStateTrigger( const int cont_no )
+    int getStateTrigger( const int& cont_no )
     {
         int state_trigger;
 
         if ( ( cont_no < 0 ) || ( cont_no > KHI_MAX_CONTROLLER ) ) { return NONE; }
         else
         {
-            state_trigger = robot_info[cont_no].state_trigger;
-            robot_info[cont_no].state_trigger = NONE;
+            state_trigger = cont_info[cont_no].state_trigger;
+            cont_info[cont_no].state_trigger = NONE;
             return state_trigger;
         }
     }
 
-    bool setStateTrigger( const int cont_no, const int state_trigger )
+    bool setStateTrigger( const int& cont_no, const int& state_trigger )
     {
         if ( !contLimitCheck( cont_no, KHI_MAX_CONTROLLER ) ) { return false; }
 
@@ -206,25 +212,17 @@ class KhiRobotDriver
         }
         else
         {
-            if ( robot_info[cont_no].state_trigger != NONE )
+            if ( cont_info[cont_no].state_trigger != NONE )
             {
                 warnPrint( "State Trigger is already done %d: %s", cont_no, KhiRobotStateTriggerName[state_trigger].c_str() );
             }
             infoPrint( "State Trigger %d: %s", cont_no, KhiRobotStateTriggerName[state_trigger].c_str() );
-            robot_info[cont_no].state_trigger = state_trigger;
+            cont_info[cont_no].state_trigger = state_trigger;
             return true;
         }
     }
 
-    bool setRobotName( const int cont_no, const std::string name )
-    {
-        if ( contLimitCheck( cont_no, KHI_MAX_CONTROLLER ) ) { return false; }
-
-        robot_info[cont_no].robot_name = name;
-        return true;
-    }
-
-    bool contLimitCheck( const int cont_no, const int limit )
+    bool contLimitCheck( const int& cont_no, const int& limit )
     {
         if ( ( cont_no < 0 ) || ( cont_no > KHI_MAX_CONTROLLER ) || ( cont_no > limit ) )
         {
@@ -237,7 +235,7 @@ class KhiRobotDriver
         }
     }
 
-    void infoPrint( const char *format, ... )
+    void infoPrint( const char* format, ... )
     {
         char msg[512] = { 0 };
         va_list ap;
@@ -248,7 +246,7 @@ class KhiRobotDriver
         ROS_INFO( "[%s] %s", driver_name.c_str(), msg );
     }
 
-    void warnPrint( const char *format, ... )
+    void warnPrint( const char* format, ... )
     {
         char msg[512] = { 0 };
         va_list ap;
@@ -259,7 +257,7 @@ class KhiRobotDriver
         ROS_WARN( "[%s] %s", driver_name.c_str(), msg );
     }
 
-    void errorPrint( const char *format, ... )
+    void errorPrint( const char* format, ... )
     {
         char msg[512] = { 0 };
         va_list ap;
@@ -270,48 +268,43 @@ class KhiRobotDriver
         ROS_ERROR( "[%s] %s", driver_name.c_str(), msg );
     }
 
-    void jointPrint( std::string name, const JointData joint )
+    void jointPrint( std::string name, const KhiRobotData& data )
     {
         char msg[512] = { 0 };
         char jt_val[16] = { 0 };
+        const double* value;
 
         snprintf( msg, sizeof(msg), "[%s]\t", name.c_str() );
-        if ( name == std::string("write") )
-        {
-            for ( int cnt = 0; cnt < joint.joint_num; cnt++ )
-            {
-                snprintf( jt_val, sizeof(jt_val), "%.3lf\t", joint.cmd[cnt] );
-                strcat( msg, jt_val );
-            }
-        }
-        else
+        for ( int ano = 0; ano < KHI_MAX_ARM; ano++ )
         {
-            for ( int cnt = 0; cnt < joint.joint_num; cnt++ )
+            if ( name == std::string("write") ) { value = data.arm[ano].cmd; }
+            else { value = data.arm[ano].pos; }
+            for ( int jt = 0; jt < data.arm[ano].jt_num; jt++ )
             {
-                snprintf( jt_val, sizeof(jt_val), "%.3lf\t", joint.pos[cnt] );
+                snprintf( jt_val, sizeof(jt_val), "%.3lf\t", value[jt] );
                 strcat( msg, jt_val );
             }
         }
         infoPrint( "[SIM]%s", msg );
     }
 
     virtual ~KhiRobotDriver() {};
-    virtual bool initialize( const int cont_no, const std::string robot_name, const double period, const JointData joint, bool in_simulation = false ) = 0;
-    virtual bool open( const int cont_no, const std::string ip_address ) = 0;
-    virtual bool close( const int cont_no ) = 0;
-    virtual bool activate( const int cont_no, JointData *joint ) = 0;
-    virtual bool hold( const int cont_no, const JointData joint ) = 0;
-    virtual bool deactivate( const int cont_no ) = 0;
-    virtual bool readData( const int cont_no, JointData *joint ) = 0;
-    virtual bool writeData( const int cont_no, JointData joint ) = 0;
-    virtual bool updateState( const int cont_no ) = 0;
-    virtual bool getPeriodDiff( const int cont_no, double *diff ) = 0;
-    virtual bool commandHandler( khi_robot_msgs::KhiRobotCmd::Request &req, khi_robot_msgs::KhiRobotCmd::Response &res ) = 0;
+    virtual bool initialize( const int& cont_no, const double& period, KhiRobotData& data, const bool in_simulation = false ) = 0;
+    virtual bool open( const int& cont_no, const std::string& ip_address, KhiRobotData& data ) = 0;
+    virtual bool close( const int& cont_no ) = 0;
+    virtual bool activate( const int& cont_no, KhiRobotData& data ) = 0;
+    virtual bool hold( const int& cont_no, const KhiRobotData& data ) = 0;
+    virtual bool deactivate( const int& cont_no, const KhiRobotData& data ) = 0;
+    virtual bool readData( const int& cont_no, KhiRobotData& data ) = 0;
+    virtual bool writeData( const int& cont_no, const KhiRobotData& data ) = 0;
+    virtual bool updateState( const int& cont_no, const KhiRobotData& data ) = 0;
+    virtual bool getPeriodDiff( const int& cont_no, double& diff ) = 0;
+    virtual bool commandHandler( khi_robot_msgs::KhiRobotCmd::Request& req, khi_robot_msgs::KhiRobotCmd::Response& res ) = 0;
 
 protected:
     bool in_simulation;
     std::string driver_name;
-    RobotInfo robot_info[KHI_MAX_CONTROLLER];
+    KhiRobotControllerInfo cont_info[KHI_MAX_CONTROLLER];
     int return_code;
     int error_code;
 };