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