機械手手實作紀錄-4.5:手套程式講解

此篇講解手套連結ROS之程式，只講解重要部分。
This article is for explaining the code that connects glove and ROS. Only for the important parts.

4.5.手套程式講解

程式於此，接下來開始講解。
Here is the code.We will begin to explain the code below.

#include "dataglovetest/dataglove_data.h"//***不知道其它的方式能不能引用？ It could be the other way to include this
#include "ihand_control/hand.h"//引入機械手格式 Include format of ihand's msg

引入msg格式。
Include format of msg.

#define PAUSE printf("\nPress Enter key to continue..."); fgetc(stdin); //define PAUSE that will let user input commands

撰寫用於輸入的等待功能。
For the function of waiting user's inputs.

ROS_INFO("\n==========ROS standby==========");
 unsigned short gloveA_scaled[18];
 //unsigned short gloveB_scaled[18];
 //---------------------------------------------------------------------------------------
 // Now begin the correction***
 char command;//for user'scommand
 float range[5][2];//5 fingers' limits,[0]=max,[1]=min
 float error_tolerance=0;
 printf("==========Begin the correction==========\n(If you want to stop now,just input Ctrl+C & Enter until glove is closed.)");
 printf("\nPlease make the gesture: \"4\",");
 PAUSE  
    fdGetSensorRawAll(pGloveA, gloveA_scaled);//原始數值 Raw data
    range[0][0]=gloveA_scaled[FD_THUMBNEAR]+error_tolerance;
    range[1][1]=gloveA_scaled[FD_INDEXNEAR]-error_tolerance;
    range[2][1]=gloveA_scaled[FD_MIDDLENEAR]-error_tolerance;
    range[3][1]=gloveA_scaled[FD_RINGNEAR]-error_tolerance;
    range[4][1]=gloveA_scaled[FD_LITTLENEAR]-error_tolerance;
    printf("OK\n");
    printf("\nPlease make the gesture: \"Thumbs up\",");
 PAUSE
    fdGetSensorRawAll(pGloveA, gloveA_scaled);//原始數值 Raw data
    range[0][1]=gloveA_scaled[FD_THUMBNEAR]-error_tolerance;
    range[1][0]=gloveA_scaled[FD_INDEXNEAR]+error_tolerance;
    range[2][0]=gloveA_scaled[FD_MIDDLENEAR]+error_tolerance;
    range[3][0]=gloveA_scaled[FD_RINGNEAR]+error_tolerance;
    range[4][0]=gloveA_scaled[FD_LITTLENEAR]+error_tolerance;
    printf("OK\n\n==========Correction is over==========");
    PAUSE

這是起始校正功能，比出指定手勢後按下Enter，此為手勢"4"以及"讚"，程式會記錄使用者當前的資料，作為mapping之標準。
This is for the function of initial calibration. It will record user's data for the standard of mapping after user make designated gestures and press "Enter". Designated gestures here is "four" and "Thumbs up".

fdGetSensorRawAll(pGloveA, gloveA_scaled);//原始數值 Raw data

                float Rthumb=(gloveA_scaled[FD_THUMBNEAR]-range[0][1]);//mapping校正 Mapping for the Correction
  float Rindex=(gloveA_scaled[FD_INDEXNEAR]-range[1][1]);
  float Rmiddle=(gloveA_scaled[FD_MIDDLENEAR]-range[2][1]);
  float Rring=(gloveA_scaled[FD_RINGNEAR]-range[3][1]);
  float Rbaby=(gloveA_scaled[FD_LITTLENEAR]-range[4][1]);
  
  ROS_INFO("A(righthand):%d",fdGetGloveHand(pGloveA)); 

處理原始資料，經過mapping後讓他變成我們要傳輸的資料。
Through mapping ,make Raw data become the data that we want to send.

ROS_INFO("A:%0.2f|%0.2f|%0.2f|%0.2f|%0.2f|%0.2f",
   //msg.LL=gloveA_scaled[FD_LITTLENEAR],
   //msg.LR=gloveA_scaled[FD_RINGNEAR],
   //msg.LM=gloveA_scaled[FD_MIDDLENEAR],
   //msg.LP=gloveA_scaled[FD_INDEXNEAR],
   //msg.LT=gloveA_scaled[FD_THUMBNEAR],
   
   msg.thumb=fabs((Rthumb/(range[0][0]-range[0][1]))*20),   
   msg.index=fabs((Rindex/(range[1][0]-range[1][1]))*20),   
   msg.middle=fabs((Rmiddle/(range[2][0]-range[2][1]))*20),   
   msg.ring=fabs((Rring/(range[3][0]-range[3][1]))*20), 
   msg.baby=fabs((Rbaby/(range[4][0]-range[4][1]))*20),
                        msg.trapezium=fabs(20-(Rthumb/(range[0][0]-range[0][1]))*20)//range for open & half status
                );
ROS_INFO("a:%d|%d|%d|%d|%d|%f",
   gloveA_scaled[FD_THUMBNEAR],
   gloveA_scaled[FD_INDEXNEAR],
   gloveA_scaled[FD_MIDDLENEAR],
   gloveA_scaled[FD_RINGNEAR],
   gloveA_scaled[FD_LITTLENEAR],
   msg.trapezium 
  ); //***  
                //printf(" >> %d\n", fdGetGesture(pGloveA));//fdGetNumGestures是顯示總共有幾種手勢可判別，通常不會變 For showing how many gestures that glove can tell
  usleep(10); // sleep 1 millisecond 
  //ss << gloveA_scaled[FD_THUMBNEAR]<<"||"<
dataglove_pub.publish(msg);
  ROS_INFO("Publish: OK");
  //ROS_INFO("Check your Right Indexfinger: %0.2f",msg.index);
  //sleep(1); // sleep unit: 1 second
  //usleep(50000);// sleep unit: 1 millisecond
  //i=0;

利用自訂msg格式，透過ROS傳輸資料。
Send data through ROS by custom format of msg.

如有問題可以留言大家一起討論，留言請勿出現不雅言語與無端謾罵喔~
If u have any question,just leave comments to me.I will answer u as soon as possible,but don't be mean to me.Thanks!