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 std::string frame_id = "leptrino";
00112 nh_private.getParam("frame_id", frame_id);
00113
00114 int i, l = 0, rt = 0;
00115 ST_RES_HEAD *stCmdHead;
00116 ST_R_DATA_GET_F *stForce;
00117 ST_R_GET_INF *stGetInfo;
00118 ST_R_LEP_GET_LIMIT* stGetLimit;
00119
00120 App_Init();
00121
00122 if (gSys.com_ok == NG)
00123 {
00124 ROS_ERROR("%s open failed\n", g_com_port.c_str());
00125 exit(0);
00126 }
00127
00128
00129 GetProductInfo();
00130 while (ros::ok())
00131 {
00132 Comm_Rcv();
00133 if (Comm_CheckRcv() != 0)
00134 {
00135 CommRcvBuff[0] = 0;
00136
00137 rt = Comm_GetRcvData(CommRcvBuff);
00138 if (rt > 0)
00139 {
00140 stGetInfo = (ST_R_GET_INF *)CommRcvBuff;
00141 stGetInfo->scFVer[F_VER_SIZE] = 0;
00142 ROS_INFO("Version: %s", stGetInfo->scFVer);
00143 stGetInfo->scSerial[SERIAL_SIZE] = 0;
00144 ROS_INFO("SerialNo: %s", stGetInfo->scSerial);
00145 stGetInfo->scPName[P_NAME_SIZE] = 0;
00146 ROS_INFO("Type: %s", stGetInfo->scPName);
00147 break;
00148 }
00149 }
00150 }
00151
00152 GetLimit();
00153 while (ros::ok())
00154 {
00155 Comm_Rcv();
00156 if (Comm_CheckRcv() != 0)
00157 {
00158 CommRcvBuff[0] = 0;
00159
00160 rt = Comm_GetRcvData(CommRcvBuff);
00161 if (rt > 0)
00162 {
00163 stGetLimit = (ST_R_LEP_GET_LIMIT *)CommRcvBuff;
00164 for (int i = 0; i < FN_Num; i++)
00165 {
00166 ROS_INFO("\tLimit[%d]: %f", i, stGetLimit->fLimit[i]);
00167 conversion_factor[i] = stGetLimit->fLimit[i] * 1e-4;
00168 }
00169 break;
00170 }
00171 }
00172 }
00173
00174 ros::Publisher force_torque_pub = nh_private.advertise<geometry_msgs::WrenchStamped>("force_torque", 1);
00175
00176 usleep(10000);
00177
00178
00179 SerialStart();
00180
00181 #if TEST_TIME
00182 double dt_sum = 0;
00183 int dt_count = 0;
00184 ros::Time start_time;
00185 #endif
00186
00187 ros::Rate rate(g_rate);
00188
00189 while (ros::ok())
00190 {
00191 Comm_Rcv();
00192 if (Comm_CheckRcv() != 0)
00193 {
00194
00195 #if TEST_TIME
00196 dt_count++;
00197 dt_sum += (ros::Time::now() - start_time).toSec();
00198 if (dt_sum >= 1.0)
00199 {
00200 ROS_INFO("Time test: read %d in %6.3f sec: %6.3f kHz", dt_count, dt_sum, (dt_count / dt_sum) * 0.001);
00201 dt_count = 0;
00202 dt_sum = 0.0;
00203 }
00204 start_time = ros::Time::now();
00205 #endif
00206
00207 memset(CommRcvBuff, 0, sizeof(CommRcvBuff));
00208 rt = Comm_GetRcvData(CommRcvBuff);
00209 if (rt > 0)
00210 {
00211 stForce = (ST_R_DATA_GET_F *)CommRcvBuff;
00212 ROS_DEBUG_THROTTLE(0.1, "%d,%d,%d,%d,%d,%d", stForce->ssForce[0], stForce->ssForce[1], stForce->ssForce[2],
00213 stForce->ssForce[3], stForce->ssForce[4], stForce->ssForce[5]);
00214
00215 geometry_msgs::WrenchStampedPtr msg(new geometry_msgs::WrenchStamped);
00216 msg->header.stamp = ros::Time::now();
00217 msg->header.frame_id = frame_id;
00218 msg->wrench.force.x = stForce->ssForce[0] * conversion_factor[0];
00219 msg->wrench.force.y = stForce->ssForce[1] * conversion_factor[1];
00220 msg->wrench.force.z = stForce->ssForce[2] * conversion_factor[2];
00221 msg->wrench.torque.x = stForce->ssForce[3] * conversion_factor[3];
00222 msg->wrench.torque.y = stForce->ssForce[4] * conversion_factor[4];
00223 msg->wrench.torque.z = stForce->ssForce[5] * conversion_factor[5];
00224 force_torque_pub.publish(msg);
00225 }
00226 }
00227 else
00228 {
00229 rate.sleep();
00230 }
00231
00232 ros::spinOnce();
00233 }
00234
00235 SerialStop();
00236 App_Close();
00237 return 0;
00238 }
00239
00240
00241
00242
00243
00244
00245
00246 void App_Init(void)
00247 {
00248 int rt;
00249
00250
00251 gSys.com_ok = NG;
00252 rt = Comm_Open(g_com_port.c_str());
00253 if (rt == OK)
00254 {
00255 Comm_Setup(460800, PAR_NON, BIT_LEN_8, 0, 0, CHR_ETX);
00256 gSys.com_ok = OK;
00257 }
00258
00259 }
00260
00261
00262
00263
00264
00265
00266
00267 void App_Close(void)
00268 {
00269 printf("Application close\n");
00270
00271 if (gSys.com_ok == OK)
00272 {
00273 Comm_Close();
00274 }
00275 }
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285 ULONG SendData(UCHAR *pucInput, USHORT usSize)
00286 {
00287 USHORT usCnt;
00288 UCHAR ucWork;
00289 UCHAR ucBCC = 0;
00290 UCHAR *pucWrite = &CommSendBuff[0];
00291 USHORT usRealSize;
00292
00293
00294 *pucWrite = CHR_DLE;
00295 pucWrite++;
00296 *pucWrite = CHR_STX;
00297 pucWrite++;
00298 usRealSize = 2;
00299
00300 for (usCnt = 0; usCnt < usSize; usCnt++)
00301 {
00302 ucWork = pucInput[usCnt];
00303 if (ucWork == CHR_DLE)
00304 {
00305 *pucWrite = CHR_DLE;
00306 pucWrite++;
00307 usRealSize++;
00308
00309 }
00310 *pucWrite = ucWork;
00311 ucBCC ^= ucWork;
00312 pucWrite++;
00313 usRealSize++;
00314 }
00315
00316 *pucWrite = CHR_DLE;
00317 pucWrite++;
00318 *pucWrite = CHR_ETX;
00319 ucBCC ^= CHR_ETX;
00320 pucWrite++;
00321 *pucWrite = ucBCC;
00322 usRealSize += 3;
00323
00324 Comm_SendData(&CommSendBuff[0], usRealSize);
00325
00326 return OK;
00327 }
00328
00329 void GetProductInfo(void)
00330 {
00331 USHORT len;
00332
00333 ROS_INFO("Get sensor information");
00334 len = 0x04;
00335 SendBuff[0] = len;
00336 SendBuff[1] = 0xFF;
00337 SendBuff[2] = CMD_GET_INF;
00338 SendBuff[3] = 0;
00339
00340 SendData(SendBuff, len);
00341 }
00342
00343 void GetLimit(void)
00344 {
00345 USHORT len;
00346
00347 ROS_INFO("Get sensor limit");
00348 len = 0x04;
00349 SendBuff[0] = len;
00350 SendBuff[1] = 0xFF;
00351 SendBuff[2] = CMD_GET_LIMIT;
00352 SendBuff[3] = 0;
00353
00354 SendData(SendBuff, len);
00355 }
00356
00357 void SerialStart(void)
00358 {
00359 USHORT len;
00360
00361 ROS_INFO("Start sensor");
00362 len = 0x04;
00363 SendBuff[0] = len;
00364 SendBuff[1] = 0xFF;
00365 SendBuff[2] = CMD_DATA_START;
00366 SendBuff[3] = 0;
00367
00368 SendData(SendBuff, len);
00369 }
00370
00371 void SerialStop(void)
00372 {
00373 USHORT len;
00374
00375 printf("Stop sensor\n");
00376 len = 0x04;
00377 SendBuff[0] = len;
00378 SendBuff[1] = 0xFF;
00379 SendBuff[2] = CMD_DATA_STOP;
00380 SendBuff[3] = 0;
00381
00382 SendData(SendBuff, len);
00383 }