00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043 #include <stdio.h>
00044 #include <string.h>
00045 #include <stdlib.h>
00046 #include <unistd.h>
00047
00048 #include <leptrino/pCommon.h>
00049 #include <leptrino/rs_comm.h>
00050 #include <leptrino/pComResInternal.h>
00051
00052 #include <ros/ros.h>
00053 #include <geometry_msgs/WrenchStamped.h>
00054
00055
00056
00057
00058 #define PRG_VER "Ver 1.0.0"
00059
00060
00061
00062
00063 typedef struct ST_SystemInfo
00064 {
00065 int com_ok;
00066 } SystemInfo;
00067
00068
00069
00070
00071 void App_Init(void);
00072 void App_Close(void);
00073 ULONG SendData(UCHAR *pucInput, USHORT usSize);
00074 void GetProductInfo(void);
00075 void GetLimit(void);
00076 void SerialStart(void);
00077 void SerialStop(void);
00078
00079
00080
00081
00082 SystemInfo gSys;
00083 UCHAR CommRcvBuff[256];
00084 UCHAR CommSendBuff[1024];
00085 UCHAR SendBuff[512];
00086 double conversion_factor[FN_Num];
00087
00088 std::string g_com_port;
00089 int g_rate;
00090
00091 #define TEST_TIME 0
00092
00093 int main(int argc, char** argv)
00094 {
00095 ros::init(argc, argv, "leptrino");
00096
00097 ros::NodeHandle nh;
00098 ros::NodeHandle nh_private("~");
00099 if (!nh_private.getParam("com_port", g_com_port))
00100 {
00101 ROS_WARN("Port is not defined, trying /dev/ttyUSB0");
00102 g_com_port = "/dev/ttyUSB0";
00103 }
00104
00105 if (!nh_private.getParam("rate", g_rate))
00106 {
00107 ROS_WARN("Rate is not defined, using maximum 1.2 kHz");
00108 g_rate = 1200;
00109 }
00110
00111 int i, l = 0, rt = 0;
00112 ST_RES_HEAD *stCmdHead;
00113 ST_R_DATA_GET_F *stForce;
00114 ST_R_GET_INF *stGetInfo;
00115 ST_R_LEP_GET_LIMIT* stGetLimit;
00116
00117 App_Init();
00118
00119 if (gSys.com_ok == NG)
00120 {
00121 ROS_ERROR("%s open failed\n", g_com_port.c_str());
00122 exit(0);
00123 }
00124
00125
00126 GetProductInfo();
00127 while (ros::ok())
00128 {
00129 Comm_Rcv();
00130 if (Comm_CheckRcv() != 0)
00131 {
00132 CommRcvBuff[0] = 0;
00133
00134 rt = Comm_GetRcvData(CommRcvBuff);
00135 if (rt > 0)
00136 {
00137 stGetInfo = (ST_R_GET_INF *)CommRcvBuff;
00138 stGetInfo->scFVer[F_VER_SIZE] = 0;
00139 ROS_INFO("Version: %s", stGetInfo->scFVer);
00140 stGetInfo->scSerial[SERIAL_SIZE] = 0;
00141 ROS_INFO("SerialNo: %s", stGetInfo->scSerial);
00142 stGetInfo->scPName[P_NAME_SIZE] = 0;
00143 ROS_INFO("Type: %s", stGetInfo->scPName);
00144 break;
00145 }
00146 }
00147 }
00148
00149 GetLimit();
00150 while (ros::ok())
00151 {
00152 Comm_Rcv();
00153 if (Comm_CheckRcv() != 0)
00154 {
00155 CommRcvBuff[0] = 0;
00156
00157 rt = Comm_GetRcvData(CommRcvBuff);
00158 if (rt > 0)
00159 {
00160 stGetLimit = (ST_R_LEP_GET_LIMIT *)CommRcvBuff;
00161 for (int i = 0; i < FN_Num; i++)
00162 {
00163 ROS_INFO("\tLimit[%d]: %f", i, stGetLimit->fLimit[i]);
00164 conversion_factor[i] = stGetLimit->fLimit[i] * 1e-4;
00165 }
00166 break;
00167 }
00168 }
00169 }
00170
00171 ros::Publisher force_torque_pub = nh_private.advertise<geometry_msgs::WrenchStamped>("force_torque", 1);
00172
00173 usleep(10000);
00174
00175
00176 SerialStart();
00177
00178 #if TEST_TIME
00179 double dt_sum = 0;
00180 int dt_count = 0;
00181 ros::Time start_time;
00182 #endif
00183
00184 ros::Rate rate(g_rate);
00185
00186 while (ros::ok())
00187 {
00188 Comm_Rcv();
00189 if (Comm_CheckRcv() != 0)
00190 {
00191
00192 #if TEST_TIME
00193 dt_count++;
00194 dt_sum += (ros::Time::now() - start_time).toSec();
00195 if (dt_sum >= 1.0)
00196 {
00197 ROS_INFO("Time test: read %d in %6.3f sec: %6.3f kHz", dt_count, dt_sum, (dt_count / dt_sum) * 0.001);
00198 dt_count = 0;
00199 dt_sum = 0.0;
00200 }
00201 start_time = ros::Time::now();
00202 #endif
00203
00204 memset(CommRcvBuff, 0, sizeof(CommRcvBuff));
00205 rt = Comm_GetRcvData(CommRcvBuff);
00206 if (rt > 0)
00207 {
00208 stForce = (ST_R_DATA_GET_F *)CommRcvBuff;
00209 ROS_DEBUG_THROTTLE(0.1, "%d,%d,%d,%d,%d,%d", stForce->ssForce[0], stForce->ssForce[1], stForce->ssForce[2],
00210 stForce->ssForce[3], stForce->ssForce[4], stForce->ssForce[5]);
00211
00212 geometry_msgs::WrenchStampedPtr msg(new geometry_msgs::WrenchStamped);
00213 msg->header.stamp = ros::Time::now();
00214 msg->wrench.force.x = stForce->ssForce[0] * conversion_factor[0];
00215 msg->wrench.force.y = stForce->ssForce[1] * conversion_factor[1];
00216 msg->wrench.force.z = stForce->ssForce[2] * conversion_factor[2];
00217 msg->wrench.torque.x = stForce->ssForce[3] * conversion_factor[3];
00218 msg->wrench.torque.y = stForce->ssForce[4] * conversion_factor[4];
00219 msg->wrench.torque.z = stForce->ssForce[5] * conversion_factor[5];
00220 force_torque_pub.publish(msg);
00221 }
00222 }
00223 else
00224 {
00225 rate.sleep();
00226 }
00227
00228 ros::spinOnce();
00229 }
00230
00231 SerialStop();
00232 App_Close();
00233 return 0;
00234 }
00235
00236
00237
00238
00239
00240
00241
00242 void App_Init(void)
00243 {
00244 int rt;
00245
00246
00247 gSys.com_ok = NG;
00248 rt = Comm_Open(g_com_port.c_str());
00249 if (rt == OK)
00250 {
00251 Comm_Setup(460800, PAR_NON, BIT_LEN_8, 0, 0, CHR_ETX);
00252 gSys.com_ok = OK;
00253 }
00254
00255 }
00256
00257
00258
00259
00260
00261
00262
00263 void App_Close(void)
00264 {
00265 printf("Application close\n");
00266
00267 if (gSys.com_ok == OK)
00268 {
00269 Comm_Close();
00270 }
00271 }
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281 ULONG SendData(UCHAR *pucInput, USHORT usSize)
00282 {
00283 USHORT usCnt;
00284 UCHAR ucWork;
00285 UCHAR ucBCC = 0;
00286 UCHAR *pucWrite = &CommSendBuff[0];
00287 USHORT usRealSize;
00288
00289
00290 *pucWrite = CHR_DLE;
00291 pucWrite++;
00292 *pucWrite = CHR_STX;
00293 pucWrite++;
00294 usRealSize = 2;
00295
00296 for (usCnt = 0; usCnt < usSize; usCnt++)
00297 {
00298 ucWork = pucInput[usCnt];
00299 if (ucWork == CHR_DLE)
00300 {
00301 *pucWrite = CHR_DLE;
00302 pucWrite++;
00303 usRealSize++;
00304
00305 }
00306 *pucWrite = ucWork;
00307 ucBCC ^= ucWork;
00308 pucWrite++;
00309 usRealSize++;
00310 }
00311
00312 *pucWrite = CHR_DLE;
00313 pucWrite++;
00314 *pucWrite = CHR_ETX;
00315 ucBCC ^= CHR_ETX;
00316 pucWrite++;
00317 *pucWrite = ucBCC;
00318 usRealSize += 3;
00319
00320 Comm_SendData(&CommSendBuff[0], usRealSize);
00321
00322 return OK;
00323 }
00324
00325 void GetProductInfo(void)
00326 {
00327 USHORT len;
00328
00329 ROS_INFO("Get sensor information");
00330 len = 0x04;
00331 SendBuff[0] = len;
00332 SendBuff[1] = 0xFF;
00333 SendBuff[2] = CMD_GET_INF;
00334 SendBuff[3] = 0;
00335
00336 SendData(SendBuff, len);
00337 }
00338
00339 void GetLimit(void)
00340 {
00341 USHORT len;
00342
00343 ROS_INFO("Get sensor limit");
00344 len = 0x04;
00345 SendBuff[0] = len;
00346 SendBuff[1] = 0xFF;
00347 SendBuff[2] = CMD_GET_LIMIT;
00348 SendBuff[3] = 0;
00349
00350 SendData(SendBuff, len);
00351 }
00352
00353 void SerialStart(void)
00354 {
00355 USHORT len;
00356
00357 ROS_INFO("Start sensor");
00358 len = 0x04;
00359 SendBuff[0] = len;
00360 SendBuff[1] = 0xFF;
00361 SendBuff[2] = CMD_DATA_START;
00362 SendBuff[3] = 0;
00363
00364 SendData(SendBuff, len);
00365 }
00366
00367 void SerialStop(void)
00368 {
00369 USHORT len;
00370
00371 printf("Stop sensor\n");
00372 len = 0x04;
00373 SendBuff[0] = len;
00374 SendBuff[1] = 0xFF;
00375 SendBuff[2] = CMD_DATA_STOP;
00376 SendBuff[3] = 0;
00377
00378 SendData(SendBuff, len);
00379 }
00380