sip.cc
Go to the documentation of this file.
00001 /*
00002  *  P2OS for ROS
00003  *  Copyright (C) 2009
00004  *     David Feil-Seifer, Brian Gerkey, Kasper Stoy, 
00005  *      Richard Vaughan, & Andrew Howard
00006  *
00007  *
00008  *  This program is free software; you can redistribute it and/or modify
00009  *  it under the terms of the GNU General Public License as published by
00010  *  the Free Software Foundation; either version 2 of the License, or
00011  *  (at your option) any later version.
00012  *
00013  *  This program is distributed in the hope that it will be useful,
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016  *  GNU General Public License for more details.
00017  *
00018  *  You should have received a copy of the GNU General Public License
00019  *  along with this program; if not, write to the Free Software
00020  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00021  *
00022  */
00023 
00024 
00025 #include <stdio.h>
00026 #include <limits.h>
00027 #include <math.h>  /* rint(3) */
00028 #include <sys/types.h>
00029 #include <stdlib.h> /* for abs() */
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   // odometry
00042 
00043   double px, py, pa;
00044 
00045   // initialize position to current offset
00046   px = this->x_offset / 1e3;
00047   py = this->y_offset / 1e3;
00048   // now transform current position by rotation if there is one
00049   // and add to offset
00050   if(this->angle_offset != 0)
00051   {
00052     double rot = DTOR(this->angle_offset);    // convert rotation to radians
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   // timestamps get set in the p2os::StandardSIPPutData fcn
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     // add rates
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   //publish transform
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   // battery
00105   data->batt.voltage = battery / 10.0;
00106 
00107   // motor state
00108   // The below will tell us if the motors are currently moving or not, it does
00109   // not tell us whether they have been enabled
00110   // data->motors.state = (status & 0x01);
00111         data->motors.state = (motors_enabled & 0x01);
00112   /*
00114   // compass
00115   memset(&(data->compass),0,sizeof(data->compass));
00116   data->compass.pos.pa = DTOR(this->compass);
00117   */
00118 
00120   // sonar
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   // gripper
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   // Reset data to false
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   // lift
00177   data->gripper.lift.dir = 0;
00178 
00179   if ((gripState & 0x10) && (gripState & 0x20) && !(gripState & 0x40))
00180   {
00181       // In this case, the lift is somewhere in between, so
00182       // must be at an intermediate carry position. Use last commanded position
00183       data->gripper.lift.state = PLAYER_ACTARRAY_ACTSTATE_IDLE;
00184       data->gripper.lift.position = lastLiftPos;
00185   }
00186   else if (gripState & 0x40)  // Moving
00187   {
00188       data->gripper.lift.state = PLAYER_ACTARRAY_ACTSTATE_MOVING;
00189       // There is no way to know where it is for sure, so use last commanded
00190       // position.
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)  // Up
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)  // Down
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    // Assume stalled
00210   {
00211       data->gripper.lift.state = PLAYER_ACTARRAY_ACTSTATE_STALLED;
00212   }
00213   // Store the last lift position
00214   lastLiftPos = data->gripper.lift.position;
00215 
00216   /*
00218   // bumper
00219   unsigned int bump_count = PlayerRobotParams[param_idx].NumFrontBumpers + PlayerRobotParams[param_idx].NumRearBumpers;
00220   if (data->bumper.bumpers_count != bump_count)
00221   {
00222     data->bumper.bumpers_count = bump_count;
00223     delete [] data->bumper.bumpers;
00224     data->bumper.bumpers = new uint8_t[bump_count];
00225   }
00226   int j = 0;
00227   for(int i=PlayerRobotParams[param_idx].NumFrontBumpers-1;i>=0;i--)
00228     data->bumper.bumpers[j++] =
00229       (unsigned char)((this->frontbumpers >> i) & 0x01);
00230   for(int i=PlayerRobotParams[param_idx].NumRearBumpers-1;i>=0;i--)
00231     data->bumper.bumpers[j++] =
00232       (unsigned char)((this->rearbumpers >> i) & 0x01);
00233   */
00234 
00236   // digital I/O
00237   data->dio.count = 8;
00238   data->dio.bits = (unsigned int)this->digin;
00239 
00241   // analog I/O
00242   //TODO: should do this smarter, based on which analog input is selected
00243   data->aio.voltages_count = (unsigned char)1;
00244   // if (!data->aio.voltages)
00245   //   data->aio.voltages = new float[1];
00246   // data->aio.voltages[0] = (this->analog / 255.0) * 5.0;
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   /* find difference in two directions and pick shortest */
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    * Remember P2OS uses little endian:
00358    * for a 2 byte short (called integer on P2OS)
00359    * byte0 is low byte, byte1 is high byte
00360    * The following code is host-machine endian independant
00361    * Also we must or (|) bytes together instead of casting to a
00362    * short * since on ARM architectures short * must be even byte aligned!
00363    * You can get away with this on a i386 since shorts * can be
00364    * odd byte aligned. But on ARM, the last bit of the pointer will be ignored!
00365    * The or'ing will work on either arch.
00366    */
00367   newxpos = ((buffer[cnt] | (buffer[cnt+1] << 8))
00368              & 0xEFFF) % 4096; /* 15 ls-bits */
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; /* 15 ls-bits */
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   //compass = buffer[cnt]*2;
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     //find the largest sonar index supplied
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     //if necessary make more space in the array and preserve existing readings
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     //update the sonar readings array with the new readings
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   // for debugging:
00487   Print();
00488   // PrintSonars();
00489 }
00490 
00500 void SIP::ParseSERAUX( unsigned char *buffer )
00501 {
00502   unsigned char type = buffer[1];
00503   if (type != SERAUX && type != SERAUX2)
00504   {
00505     // Really should never get here...
00506     ROS_ERROR("Attempt to parse non SERAUX packet as serial data.\n");
00507     return;
00508   }
00509 
00510   int len  = (int)buffer[0]-3;          // Get the string length
00511 
00512   /* First thing is to find the beginning of last full packet (if possible).
00513   ** If there are less than CMUCAM_MESSAGE_LEN*2-1 bytes (19), we're not
00514   ** guaranteed to have a full packet.  If less than CMUCAM_MESSAGE_LEN
00515   ** bytes, it is impossible to have a full packet.
00516   ** To find the beginning of the last full packet, search between bytes
00517   ** len-17 and len-8 (inclusive) for the start flag (255).
00518   */
00519   int ix;
00520   for (ix = (len>18 ? len-17 : 2); ix<=len-8; ix++)
00521     if (buffer[ix] == 255)
00522       break;            // Got it!
00523   if (len < 10 || ix > len-8) {
00524     ROS_ERROR("Failed to get a full blob tracking packet.\n");
00525     return;
00526   }
00527 
00528   // There is a special 'S' message containing the tracking color info
00529   if (buffer[ix+1] == 'S')
00530   {
00531      // Color information (track color)
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   // Tracking packets with centroid info are designated with a 'M'
00541   if (buffer[ix+1] == 'M')
00542   {
00543      // Now it's easy.  Just parse the packet.
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      // Xiaoquan Fu's calculation for blob area (max 11297).
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 // Parse a set of gyro measurements.  The buffer is formatted thusly:
00561 //     length (2 bytes), type (1 byte), count (1 byte)
00562 // followed by <count> pairs of the form:
00563 //     rate (2 bytes), temp (1 byte)
00564 // <rate> falls in [0,1023]; less than 512 is CCW rotation and greater
00565 // than 512 is CW
00566 void
00567 SIP::ParseGyro(unsigned char* buffer)
00568 {
00569   // Get the message length (account for the type byte and the 2-byte
00570   // checksum)
00571   int len  = (int)buffer[0]-3;
00572 
00573   unsigned char type = buffer[1];
00574   if(type != GYROPAC)
00575   {
00576     // Really should never get here...
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   // get count
00588   int count = (int)buffer[2];
00589 
00590   // sanity check
00591   if((len-1) != (count*3))
00592   {
00593     ROS_DEBUG("Mismatch between gyro measurement count and packet length");
00594     return;
00595   }
00596 
00597   // Actually handle the rate values.  Any number of things could (and
00598   // probably should) be done here, like filtering, calibration, conversion
00599   // from the gyro's arbitrary units to something meaningful, etc.
00600   //
00601   // As a first cut, we'll just average all the rate measurements in this
00602   // set, and ignore the temperatures.
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   // store the result for sending
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;              // Power is on
00641         else
00642                 armPowerOn = false;             // Power is off
00643 
00644         if (status & 0x02)
00645                 armConnected = true;    // Connection is established
00646         else
00647                 armConnected = false;   // Connection is not established
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         // Copy the version string
00683         if (armVersionString)
00684                 free (armVersionString);
00685         // strndup() isn't available everywhere (e.g., Darwin)
00686         //armVersionString = strndup ((char*) &buffer[2], length);              // Can't be any bigger than length
00687         armVersionString = (char*)calloc(length+1,sizeof(char));
00688         assert(armVersionString);
00689         strncpy(armVersionString,(char*)&buffer[2], length);
00690         int dataOffset = strlen (armVersionString) + 3;         // +1 for packet size byte, +1 for packet ID, +1 for null byte
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 }


p2os_driver
Author(s): Hunter Allen , David Feil-Seifer , Aris Synodinos , Brian Gerkey, Kasper Stoy, Richard Vaughan, Andrew Howard, Tucker Hermans, ActivMedia Robotics LLC, MobileRobots Inc.
autogenerated on Mon Oct 6 2014 03:12:45