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 #include <stdio.h>
00026 #include <limits.h>
00027 #include <math.h>
00028 #include <sys/types.h>
00029 #include <stdlib.h>
00030 #include <unistd.h>
00031
00032 #include <p2os_driver/sip.h>
00033 #include <tf/tf.h>
00034 #include <tf/transform_datatypes.h>
00035 #include <sstream>
00036 #include <boost/assign/list_of.hpp>
00037
00038 void SIP::FillStandard(ros_p2os_data_t* data)
00039 {
00041
00042
00043 double px, py, pa;
00044
00045
00046 px = this->x_offset / 1e3;
00047 py = this->y_offset / 1e3;
00048
00049
00050 if(this->angle_offset != 0)
00051 {
00052 double rot = DTOR(this->angle_offset);
00053 px += ((this->xpos/1e3) * cos(rot) -
00054 (this->ypos/1e3) * sin(rot));
00055 py += ((this->xpos/1e3) * sin(rot) +
00056 (this->ypos/1e3) * cos(rot));
00057 pa = DTOR(this->angle_offset + angle);
00058 }
00059 else
00060 {
00061 px += this->xpos / 1e3;
00062 py += this->ypos / 1e3;
00063 pa = DTOR(this->angle);
00064 }
00065
00066
00067 data->position.header.frame_id = "odom";
00068 data->position.child_frame_id = "base_link";
00069
00070 data->position.pose.pose.position.x = px;
00071 data->position.pose.pose.position.y = py;
00072 data->position.pose.pose.position.z = 0.0;
00073 data->position.pose.pose.orientation = tf::createQuaternionMsgFromYaw(pa);
00074
00075
00076 data->position.twist.twist.linear.x = ((lvel + rvel) / 2) / 1e3;
00077 data->position.twist.twist.linear.y = 0.0;
00078 data->position.twist.twist.angular.z = ((double)(rvel-lvel)/(2.0/PlayerRobotParams[param_idx].DiffConvFactor));
00079
00080
00081 data->position.pose.covariance = boost::assign::list_of (1e-3) (0) (0) (0) (0) (0)
00082 (0) (1e-3) (0) (0) (0) (0)
00083 (0) (0) (1e6) (0) (0) (0)
00084 (0) (0) (0) (1e6) (0) (0)
00085 (0) (0) (0) (0) (1e6) (0)
00086 (0) (0) (0) (0) (0) (1e3) ;
00087
00088 data->position.twist.covariance = boost::assign::list_of (1e-3) (0) (0) (0) (0) (0)
00089 (0) (1e-3) (0) (0) (0) (0)
00090 (0) (0) (1e6) (0) (0) (0)
00091 (0) (0) (0) (1e6) (0) (0)
00092 (0) (0) (0) (0) (1e6) (0)
00093 (0) (0) (0) (0) (0) (1e3) ;
00094
00095
00096
00097 data->odom_trans.header = data->position.header;
00098 data->odom_trans.child_frame_id = data->position.child_frame_id;
00099 data->odom_trans.transform.translation.x = px;
00100 data->odom_trans.transform.translation.y = py;
00101 data->odom_trans.transform.translation.z = 0;
00102 data->odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pa);
00103
00104
00105 data->batt.voltage = battery / 10.0;
00106
00107
00108
00109
00110
00111 data->motors.state = (motors_enabled & 0x01);
00112
00114
00115
00116
00117
00118
00120
00121 data->sonar.ranges_count = static_cast<int>(sonarreadings);
00122 data->sonar.ranges.clear();
00123 for(int i=0; i < data->sonar.ranges_count; i++)
00124 data->sonar.ranges.push_back(sonars[i] / 1e3);
00125
00127
00128 unsigned char gripState = timer;
00129 if ((gripState & 0x01) && (gripState & 0x02) && !(gripState & 0x04))
00130 {
00131 data->gripper.grip.state = PLAYER_GRIPPER_STATE_ERROR;
00132 data->gripper.grip.dir = 0;
00133 }
00134 else if(gripState & 0x04)
00135 {
00136 data->gripper.grip.state = PLAYER_GRIPPER_STATE_MOVING;
00137 if(gripState & 0x01)
00138 data->gripper.grip.dir = 1;
00139 if(gripState & 0x02)
00140 data->gripper.grip.dir = -1;
00141 }
00142 else if(gripState & 0x01)
00143 {
00144 data->gripper.grip.state = PLAYER_GRIPPER_STATE_OPEN;
00145 data->gripper.grip.dir = 0;
00146 }
00147 else if(gripState & 0x02)
00148 {
00149 data->gripper.grip.state = PLAYER_GRIPPER_STATE_CLOSED;
00150 data->gripper.grip.dir = 0;
00151 }
00152
00153
00154 data->gripper.grip.inner_beam = false;
00155 data->gripper.grip.outer_beam = false;
00156 data->gripper.grip.left_contact = false;
00157 data->gripper.grip.right_contact = false;
00158
00159 if (digin & 0x08)
00160 {
00161 data->gripper.grip.inner_beam = true;
00162 }
00163 if (digin & 0x04)
00164 {
00165 data->gripper.grip.outer_beam = true;
00166 }
00167 if (!(digin & 0x10))
00168 {
00169 data->gripper.grip.left_contact = true;
00170 }
00171 if (!(digin & 0x20))
00172 {
00173 data->gripper.grip.right_contact = true;
00174 }
00175
00176
00177 data->gripper.lift.dir = 0;
00178
00179 if ((gripState & 0x10) && (gripState & 0x20) && !(gripState & 0x40))
00180 {
00181
00182
00183 data->gripper.lift.state = PLAYER_ACTARRAY_ACTSTATE_IDLE;
00184 data->gripper.lift.position = lastLiftPos;
00185 }
00186 else if (gripState & 0x40)
00187 {
00188 data->gripper.lift.state = PLAYER_ACTARRAY_ACTSTATE_MOVING;
00189
00190
00191 data->gripper.lift.position = lastLiftPos;
00192 if (gripState & 0x10)
00193 data->gripper.lift.dir = 1;
00194 else if (gripState & 0x20)
00195 data->gripper.lift.dir = -1;
00196 }
00197 else if (gripState & 0x10)
00198 {
00199 data->gripper.lift.state = PLAYER_ACTARRAY_ACTSTATE_IDLE;
00200 data->gripper.lift.position = 1.0f;
00201 data->gripper.lift.dir = 0;
00202 }
00203 else if (gripState & 0x20)
00204 {
00205 data->gripper.lift.state = PLAYER_ACTARRAY_ACTSTATE_IDLE;
00206 data->gripper.lift.position = 0.0f;
00207 data->gripper.lift.dir = 0;
00208 }
00209 else
00210 {
00211 data->gripper.lift.state = PLAYER_ACTARRAY_ACTSTATE_STALLED;
00212 }
00213
00214 lastLiftPos = data->gripper.lift.position;
00215
00216
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00236
00237 data->dio.count = 8;
00238 data->dio.bits = (unsigned int)this->digin;
00239
00241
00242
00243 data->aio.voltages_count = (unsigned char)1;
00244
00245
00246
00247 data->aio.voltages.clear();
00248 data->aio.voltages.push_back((this->analog / 255.0) * 5.0);
00249 }
00250
00251 int SIP::PositionChange( unsigned short from, unsigned short to )
00252 {
00253 int diff1, diff2;
00254
00255
00256 if ( to > from ) {
00257 diff1 = to - from;
00258 diff2 = - ( from + 4096 - to );
00259 }
00260 else {
00261 diff1 = to - from;
00262 diff2 = 4096 - from + to;
00263 }
00264
00265 if ( abs(diff1) < abs(diff2) )
00266 return(diff1);
00267 else
00268 return(diff2);
00269
00270 }
00271
00272 void SIP::Print()
00273 {
00274 int i;
00275
00276 ROS_DEBUG("lwstall:%d rwstall:%d\n", lwstall, rwstall);
00277
00278 std::stringstream front_bumper_info;
00279 for(int i=0;i<5;i++) {
00280 front_bumper_info << " "
00281 << static_cast<int>((frontbumpers >> i) & 0x01 );
00282 }
00283 ROS_DEBUG("Front bumpers:%s", front_bumper_info.str().c_str());
00284 std::stringstream rear_bumper_info;
00285 for(int i=0;i<5;i++) {
00286 rear_bumper_info << " "
00287 << static_cast<int>((rearbumpers >> i) & 0x01 );
00288 }
00289 ROS_DEBUG("Rear bumpers:%s", rear_bumper_info.str().c_str());
00290
00291 ROS_DEBUG("status: 0x%x analog: %d param_id: %d ", status, analog, param_idx);
00292 std::stringstream status_info;
00293 for(i=0;i<11;i++) {
00294 status_info << " "
00295 << static_cast<int>((status >> (7-i) ) & 0x01);
00296 }
00297 ROS_DEBUG("status:%s", status_info.str().c_str());
00298 std::stringstream digin_info;
00299 for(i=0;i<8;i++) {
00300 digin_info << " "
00301 << static_cast<int>((digin >> (7-i) ) & 0x01);
00302 }
00303 ROS_DEBUG("digin:%s", digin_info.str().c_str());
00304 std::stringstream digout_info;
00305 for(i=0;i<8;i++) {
00306 digout_info << " "
00307 << static_cast<int>((digout >> (7-i) ) & 0x01);
00308 }
00309 ROS_DEBUG("digout:%s", digout_info.str().c_str());
00310 ROS_DEBUG("battery: %d compass: %d sonarreadings: %d\n", battery,
00311 compass, sonarreadings);
00312 ROS_DEBUG("xpos: %d ypos:%d ptu:%hu timer:%hu\n", xpos, ypos, ptu, timer);
00313 ROS_DEBUG("angle: %d lvel: %d rvel: %d control: %d\n", angle, lvel,
00314 rvel, control);
00315
00316 PrintSonars();
00317 PrintArmInfo ();
00318 PrintArm ();
00319 }
00320
00321 void SIP::PrintSonars()
00322 {
00323 if(sonarreadings <= 0)
00324 return;
00325 std::stringstream sonar_info;
00326 for(int i = 0; i < sonarreadings; i++){
00327 sonar_info << " " << static_cast<int>(sonars[i]);
00328 }
00329 ROS_DEBUG("Sonars: %s", sonar_info.str().c_str());
00330 }
00331
00332 void SIP::PrintArm ()
00333 {
00334 ROS_DEBUG ("Arm power is %s\tArm is %sconnected\n", (armPowerOn ? "on" : "off"), (armConnected ? "" : "not "));
00335 ROS_DEBUG ("Arm joint status:\n");
00336 for (int ii = 0; ii < 6; ii++)
00337 ROS_DEBUG ("Joint %d %s %d\n", ii + 1, (armJointMoving[ii] ? "Moving " : "Stopped"), armJointPos[ii]);
00338 }
00339
00340 void SIP::PrintArmInfo ()
00341 {
00342 ROS_DEBUG ("Arm version:\t%s\n", armVersionString);
00343 ROS_DEBUG ("Arm has %d joints:\n", armNumJoints);
00344 ROS_DEBUG (" |\tSpeed\tHome\tMin\tCentre\tMax\tTicks/90\n");
00345 for (int ii = 0; ii < armNumJoints; ii++)
00346 ROS_DEBUG ("%d |\t%d\t%d\t%d\t%d\t%d\t%d\n", ii, armJoints[ii].speed, armJoints[ii].home, armJoints[ii].min, armJoints[ii].centre, armJoints[ii].max, armJoints[ii].ticksPer90);
00347 }
00348
00349 void SIP::ParseStandard( unsigned char *buffer )
00350 {
00351 int cnt = 0, change;
00352 unsigned short newxpos, newypos;
00353
00354 status = buffer[cnt];
00355 cnt += sizeof(unsigned char);
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367 newxpos = ((buffer[cnt] | (buffer[cnt+1] << 8))
00368 & 0xEFFF) % 4096;
00369
00370 if (xpos!=INT_MAX) {
00371 change = (int) rint(PositionChange( rawxpos, newxpos ) *
00372 PlayerRobotParams[param_idx].DistConvFactor);
00373 if (abs(change)>100)
00374 ROS_DEBUG("invalid odometry change [%d]; odometry values are tainted", change);
00375 else
00376 xpos += change;
00377 }
00378 else
00379 xpos = 0;
00380 rawxpos = newxpos;
00381 cnt += sizeof(short);
00382
00383 newypos = ((buffer[cnt] | (buffer[cnt+1] << 8))
00384 & 0xEFFF) % 4096;
00385
00386 if (ypos!=INT_MAX) {
00387 change = (int) rint(PositionChange( rawypos, newypos ) *
00388 PlayerRobotParams[param_idx].DistConvFactor);
00389 if (abs(change)>100)
00390 ROS_DEBUG("invalid odometry change [%d]; odometry values are tainted", change);
00391 else
00392 ypos += change;
00393 }
00394 else
00395 ypos = 0;
00396 rawypos = newypos;
00397 cnt += sizeof(short);
00398
00399 angle = (short)
00400 rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
00401 PlayerRobotParams[param_idx].AngleConvFactor * 180.0/M_PI);
00402 cnt += sizeof(short);
00403
00404 lvel = (short)
00405 rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
00406 PlayerRobotParams[param_idx].VelConvFactor);
00407 cnt += sizeof(short);
00408
00409 rvel = (short)
00410 rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
00411 PlayerRobotParams[param_idx].VelConvFactor);
00412 cnt += sizeof(short);
00413
00414 battery = buffer[cnt];
00415 cnt += sizeof(unsigned char);
00416 ROS_DEBUG( "battery value: %d", battery );
00417
00418 lwstall = buffer[cnt] & 0x01;
00419 rearbumpers = buffer[cnt] >> 1;
00420 cnt += sizeof(unsigned char);
00421
00422 rwstall = buffer[cnt] & 0x01;
00423 frontbumpers = buffer[cnt] >> 1;
00424 cnt += sizeof(unsigned char);
00425
00426 control = (short)
00427 rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
00428 PlayerRobotParams[param_idx].AngleConvFactor);
00429 cnt += sizeof(short);
00430
00431 ptu = (buffer[cnt] | (buffer[cnt+1] << 8));
00432 motors_enabled = buffer[cnt];
00433 sonar_flag = buffer[cnt+1];
00434 cnt += sizeof(short);
00435
00436
00437 if(buffer[cnt] != 255 && buffer[cnt] != 0 && buffer[cnt] != 181)
00438 compass = (buffer[cnt]-1)*2;
00439 cnt += sizeof(unsigned char);
00440
00441 unsigned char numSonars=buffer[cnt];
00442 cnt+=sizeof(unsigned char);
00443
00444 if(numSonars > 0)
00445 {
00446
00447 unsigned char maxSonars=sonarreadings;
00448 for(unsigned char i=0;i<numSonars;i++)
00449 {
00450 unsigned char sonarIndex=buffer[cnt+i*(sizeof(unsigned char)+sizeof(unsigned short))];
00451 if((sonarIndex+1)>maxSonars) maxSonars=sonarIndex+1;
00452 }
00453
00454
00455 if(maxSonars>sonarreadings)
00456 {
00457 unsigned short *newSonars=new unsigned short[maxSonars];
00458 for(unsigned char i=0;i<sonarreadings;i++)
00459 newSonars[i]=sonars[i];
00460 if(sonars!=NULL) delete[] sonars;
00461 sonars=newSonars;
00462 sonarreadings=maxSonars;
00463 }
00464
00465
00466 for(unsigned char i=0;i<numSonars;i++)
00467 {
00468 sonars[buffer[cnt]]= (unsigned short)
00469 rint((buffer[cnt+1] | (buffer[cnt+2] << 8)) *
00470 PlayerRobotParams[param_idx].RangeConvFactor);
00471 cnt+=sizeof(unsigned char)+sizeof(unsigned short);
00472 }
00473 }
00474
00475 timer = (buffer[cnt] | (buffer[cnt+1] << 8));
00476 cnt += sizeof(short);
00477
00478 analog = buffer[cnt];
00479 cnt += sizeof(unsigned char);
00480
00481 digin = buffer[cnt];
00482 cnt += sizeof(unsigned char);
00483
00484 digout = buffer[cnt];
00485 cnt += sizeof(unsigned char);
00486
00487 Print();
00488
00489 }
00490
00500 void SIP::ParseSERAUX( unsigned char *buffer )
00501 {
00502 unsigned char type = buffer[1];
00503 if (type != SERAUX && type != SERAUX2)
00504 {
00505
00506 ROS_ERROR("Attempt to parse non SERAUX packet as serial data.\n");
00507 return;
00508 }
00509
00510 int len = (int)buffer[0]-3;
00511
00512
00513
00514
00515
00516
00517
00518
00519 int ix;
00520 for (ix = (len>18 ? len-17 : 2); ix<=len-8; ix++)
00521 if (buffer[ix] == 255)
00522 break;
00523 if (len < 10 || ix > len-8) {
00524 ROS_ERROR("Failed to get a full blob tracking packet.\n");
00525 return;
00526 }
00527
00528
00529 if (buffer[ix+1] == 'S')
00530 {
00531
00532 ROS_DEBUG("Tracking color (RGB): %d %d %d\n"
00533 " with variance: %d %d %d\n",
00534 buffer[ix+2], buffer[ix+3], buffer[ix+4],
00535 buffer[ix+5], buffer[ix+6], buffer[ix+7]);
00536 blobcolor = buffer[ix+2]<<16 | buffer[ix+3]<<8 | buffer[ix+4];
00537 return;
00538 }
00539
00540
00541 if (buffer[ix+1] == 'M')
00542 {
00543
00544 blobmx = buffer[ix+2];
00545 blobmy = buffer[ix+3];
00546 blobx1 = buffer[ix+4];
00547 bloby1 = buffer[ix+5];
00548 blobx2 = buffer[ix+6];
00549 bloby2 = buffer[ix+7];
00550 blobconf = buffer[ix+9];
00551
00552 blobarea = (bloby2 - bloby1 +1)*(blobx2 - blobx1 + 1)*blobconf/255;
00553 return;
00554 }
00555
00556 ROS_ERROR("Unknown blob tracker packet type: %c\n", buffer[ix+1]);
00557 return;
00558 }
00559
00560
00561
00562
00563
00564
00565
00566 void
00567 SIP::ParseGyro(unsigned char* buffer)
00568 {
00569
00570
00571 int len = (int)buffer[0]-3;
00572
00573 unsigned char type = buffer[1];
00574 if(type != GYROPAC)
00575 {
00576
00577 ROS_ERROR("Attempt to parse non GYRO packet as gyro data.\n");
00578 return;
00579 }
00580
00581 if(len < 1)
00582 {
00583 ROS_DEBUG("Couldn't get gyro measurement count");
00584 return;
00585 }
00586
00587
00588 int count = (int)buffer[2];
00589
00590
00591 if((len-1) != (count*3))
00592 {
00593 ROS_DEBUG("Mismatch between gyro measurement count and packet length");
00594 return;
00595 }
00596
00597
00598
00599
00600
00601
00602
00603 float ratesum = 0;
00604 int bufferpos = 3;
00605 unsigned short rate;
00606 unsigned char temp;
00607 for(int i=0; i<count; i++)
00608 {
00609 rate = (unsigned short)(buffer[bufferpos++]);
00610 rate |= buffer[bufferpos++] << 8;
00611 temp = bufferpos++;
00612
00613 ratesum += rate;
00614 }
00615
00616 int32_t average_rate = (int32_t)rint(ratesum / (float)count);
00617
00618
00619 gyro_rate = average_rate;
00620 }
00621
00622 void SIP::ParseArm (unsigned char *buffer)
00623 {
00624 int length = (int) buffer[0] - 2;
00625
00626 if (buffer[1] != ARMPAC)
00627 {
00628 ROS_ERROR("Attempt to parse a non ARM packet as arm data.\n");
00629 return;
00630 }
00631
00632 if (length < 1 || length != 9)
00633 {
00634 ROS_DEBUG ("ARMpac length incorrect size");
00635 return;
00636 }
00637
00638 unsigned char status = buffer[2];
00639 if (status & 0x01)
00640 armPowerOn = true;
00641 else
00642 armPowerOn = false;
00643
00644 if (status & 0x02)
00645 armConnected = true;
00646 else
00647 armConnected = false;
00648
00649 unsigned char motionStatus = buffer[3];
00650 if (motionStatus & 0x01)
00651 armJointMoving[0] = true;
00652 if (motionStatus & 0x02)
00653 armJointMoving[1] = true;
00654 if (motionStatus & 0x04)
00655 armJointMoving[2] = true;
00656 if (motionStatus & 0x08)
00657 armJointMoving[3] = true;
00658 if (motionStatus & 0x10)
00659 armJointMoving[4] = true;
00660 if (motionStatus & 0x20)
00661 armJointMoving[5] = true;
00662
00663 memcpy (armJointPos, &buffer[4], 6);
00664 memset (armJointPosRads, 0, 6 * sizeof (double));
00665 }
00666
00667 void SIP::ParseArmInfo (unsigned char *buffer)
00668 {
00669 int length = (int) buffer[0] - 2;
00670 if (buffer[1] != ARMINFOPAC)
00671 {
00672 ROS_ERROR ("Attempt to parse a non ARMINFO packet as arm info.\n");
00673 return;
00674 }
00675
00676 if (length < 1)
00677 {
00678 ROS_DEBUG ("ARMINFOpac length bad size");
00679 return;
00680 }
00681
00682
00683 if (armVersionString)
00684 free (armVersionString);
00685
00686
00687 armVersionString = (char*)calloc(length+1,sizeof(char));
00688 assert(armVersionString);
00689 strncpy(armVersionString,(char*)&buffer[2], length);
00690 int dataOffset = strlen (armVersionString) + 3;
00691
00692 armNumJoints = buffer[dataOffset];
00693 if (armJoints)
00694 delete[] armJoints;
00695 if (armNumJoints <= 0)
00696 return;
00697 armJoints = new ArmJoint[armNumJoints];
00698 dataOffset += 1;
00699 for (int ii = 0; ii < armNumJoints; ii++)
00700 {
00701 armJoints[ii].speed = buffer[dataOffset + (ii * 6)];
00702 armJoints[ii].home = buffer[dataOffset + (ii * 6) + 1];
00703 armJoints[ii].min = buffer[dataOffset + (ii * 6) + 2];
00704 armJoints[ii].centre = buffer[dataOffset + (ii * 6) + 3];
00705 armJoints[ii].max = buffer[dataOffset + (ii * 6) + 4];
00706 armJoints[ii].ticksPer90 = buffer[dataOffset + (ii * 6) + 5];
00707 }
00708 }