sip.cc

机器人仿真软件

      ypos += change;
  }
  else
    ypos = 0;
  rawypos = newypos;
  cnt += sizeof(short);

  angle = (short)
    rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
	 PlayerRobotParams[param_idx].AngleConvFactor * 180.0/M_PI);
  cnt += sizeof(short);

  lvel = (short)
    rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
	 PlayerRobotParams[param_idx].VelConvFactor);
  cnt += sizeof(short);

  rvel = (short)
    rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
	 PlayerRobotParams[param_idx].VelConvFactor);
  cnt += sizeof(short);

  battery = buffer[cnt];
  cnt += sizeof(unsigned char);
  
  lwstall = buffer[cnt] & 0x01;
  rearbumpers = buffer[cnt] >> 1;
  cnt += sizeof(unsigned char);
  
  rwstall = buffer[cnt] & 0x01;
  frontbumpers = buffer[cnt] >> 1;
  cnt += sizeof(unsigned char);

  control = (short)
    rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
	 PlayerRobotParams[param_idx].AngleConvFactor);
  cnt += sizeof(short);

  ptu = (buffer[cnt] | (buffer[cnt+1] << 8));
  cnt += sizeof(short);

  //compass = buffer[cnt]*2;
  if(buffer[cnt] != 255 && buffer[cnt] != 0 && buffer[cnt] != 181)
    compass = (buffer[cnt]-1)*2;
  cnt += sizeof(unsigned char);

  sonarreadings = buffer[cnt];
  cnt += sizeof(unsigned char);

  //printf("%hu sonar readings:\n", sonarreadings);
  for(unsigned char i = 0;i < sonarreadings;i++) {
    sonars[buffer[cnt]]=   (unsigned short)
      rint((buffer[cnt+1] | (buffer[cnt+2] << 8)) *
	   PlayerRobotParams[param_idx].RangeConvFactor);
    //printf("%d %hu:",buffer[cnt],*((unsigned short *)&buffer[cnt+1]));
    //     
    //printf("%hu %hu %hu\n", buffer[cnt], buffer[cnt+1], buffer[cnt+2]);
    //printf("index %d value %hu\n", buffer[cnt], sonars[buffer[cnt]]);
    cnt += 3*sizeof(unsigned char);
  }
  //printf("\n");

  timer = (buffer[cnt] | (buffer[cnt+1] << 8));
  cnt += sizeof(short);

  analog = buffer[cnt];
  cnt += sizeof(unsigned char);

  digin = buffer[cnt];
  cnt += sizeof(unsigned char);

  digout = buffer[cnt];
  cnt += sizeof(unsigned char);
  // for debugging:
  //Print();
}


/** Parse a SERAUX SIP packet.  For a CMUcam, this will have blob
 **  tracking messages in the format (all one-byte values, no spaces): 
 **
 **      255 M mx my x1 y1 x2 y2 pixels confidence  (10-bytes)
 **
 ** Or color info messages of the format:
 **
 **      255 S Rval Gval Bval Rvar Gvar Bvar    (8-bytes)
 */
void SIP::ParseSERAUX( unsigned char *buffer ) 
{
  unsigned char type = buffer[1];
  if (type != SERAUX && type != SERAUX2)
  {
    // Really should never get here...
    printf("ERROR: Attempt to parse non SERAUX packet as serial data.\n");
    return;
  }

  int len  = (int)buffer[0]-3;		// Get the string length

  /* First thing is to find the beginning of last full packet (if possible).
  ** If there are less than CMUCAM_MESSAGE_LEN*2-1 bytes (19), we're not
  ** guaranteed to have a full packet.  If less than CMUCAM_MESSAGE_LEN
  ** bytes, it is impossible to have a full packet.
  ** To find the beginning of the last full packet, search between bytes
  ** len-17 and len-8 (inclusive) for the start flag (255).
  */
  int ix;
  for (ix = (len>18 ? len-17 : 2); ix<=len-8; ix++)
    if (buffer[ix] == 255)
      break;		// Got it!
  if (len < 10 || ix > len-8) {
    printf("ERROR: Failed to get a full blob tracking packet.\n");
    return;
  }

  // There is a special 'S' message containing the tracking color info
  if (buffer[ix+1] == 'S')
  {
     // Color information (track color)
     printf("Tracking color (RGB):  %d %d %d\n"
            "       with variance:  %d %d %d\n",
              buffer[ix+2], buffer[ix+3], buffer[ix+4],
              buffer[ix+5], buffer[ix+6], buffer[ix+7]);
     blobcolor = buffer[ix+2]<<16 | buffer[ix+3]<<8 | buffer[ix+4];
     return;
  }

  // Tracking packets with centroid info are designated with a 'M'
  if (buffer[ix+1] == 'M')
  {
     // Now it's easy.  Just parse the packet.
     blobmx	= buffer[ix+2];
     blobmy	= buffer[ix+3];
     blobx1	= buffer[ix+4];
     bloby1	= buffer[ix+5];
     blobx2	= buffer[ix+6];
     bloby2	= buffer[ix+7];
     blobconf	= buffer[ix+9];
     // Xiaoquan Fu's calculation for blob area (max 11297).
     blobarea	= (bloby2 - bloby1 +1)*(blobx2 - blobx1 + 1)*blobconf/255;
     return;
  }

  printf("ERROR: Unknown blob tracker packet type: %c\n", buffer[ix+1]); 
  return;
}

// Parse a set of gyro measurements.  The buffer is formatted thusly:
//     length (2 bytes), type (1 byte), count (1 byte)
// followed by <count> pairs of the form:
//     rate (2 bytes), temp (1 byte)
// <rate> falls in [0,1023]; less than 512 is CCW rotation and greater
// than 512 is CW
void
SIP::ParseGyro(unsigned char* buffer)
{
  // Get the message length (account for the type byte and the 2-byte
  // checksum)
  int len  = (int)buffer[0]-3;

  unsigned char type = buffer[1];
  if(type != GYROPAC)
  {
    // Really should never get here...
    PLAYER_ERROR("ERROR: Attempt to parse non GYRO packet as gyro data.\n");
    return;
  }

  if(len < 1)
  {
    PLAYER_WARN("Couldn't get gyro measurement count");
    return;
  }

  // get count
  int count = (int)buffer[2];

  // sanity check
  if((len-1) != (count*3))
  {
    PLAYER_WARN("Mismatch between gyro measurement count and packet length");
    return;
  }

  // Actually handle the rate values.  Any number of things could (and
  // probably should) be done here, like filtering, calibration, conversion
  // from the gyro's arbitrary units to something meaningful, etc.
  //
  // As a first cut, we'll just average all the rate measurements in this 
  // set, and ignore the temperatures.
  float ratesum = 0;
  int bufferpos = 3;
  unsigned short rate;
  unsigned char temp;
  for(int i=0; i<count; i++)
  {
    rate = (unsigned short)(buffer[bufferpos++]);
    rate |= buffer[bufferpos++] << 8;
    temp = bufferpos++;

    ratesum += rate;
  }

  int32_t average_rate = (int32_t)rint(ratesum / (float)count);

  // store the result for sending
  gyro_rate = average_rate;
}

void SIP::ParseArm (unsigned char *buffer)
{
	int length = (int) buffer[0] - 2;

	if (buffer[1] != ARMPAC)
	{
		PLAYER_ERROR ("ERROR: Attempt to parse a non ARM packet as arm data.\n");
		return;
	}

	if (length < 1 || length != 9)
	{
		PLAYER_WARN ("ARMpac length incorrect size");
		return;
	}

	unsigned char status = buffer[2];
	if (status & 0x01)
		armPowerOn = true;		// Power is on
	else
		armPowerOn = false;		// Power is off

	if (status & 0x02)
		armConnected = true;	// Connection is established
	else
		armConnected = false;	// Connection is not established

	unsigned char motionStatus = buffer[3];
	if (motionStatus & 0x01)
		armJointMoving[0] = true;
	if (motionStatus & 0x02)
		armJointMoving[1] = true;
	if (motionStatus & 0x04)
		armJointMoving[2] = true;
	if (motionStatus & 0x08)
		armJointMoving[3] = true;
	if (motionStatus & 0x10)
		armJointMoving[4] = true;
	if (motionStatus & 0x20)
		armJointMoving[5] = true;

	memcpy (armJointPos, &buffer[4], 6);
	memset (armJointPosRads, 0, 6 * sizeof (double));
}

void SIP::ParseArmInfo (unsigned char *buffer)
{
	int length = (int) buffer[0] - 2;
	if (buffer[1] != ARMINFOPAC)
	{
		PLAYER_ERROR ("ERROR: Attempt to parse a non ARMINFO packet as arm info.\n");
		return;
	}

	if (length < 1)
	{
		PLAYER_WARN ("ARMINFOpac length bad size");
		return;
	}

	// Copy the version string
	if (armVersionString)
		free (armVersionString);
        // strndup() isn't available everywhere (e.g., Darwin)
	//armVersionString = strndup ((char*) &buffer[2], length);		// Can't be any bigger than length
        armVersionString = (char*)calloc(length+1,sizeof(char));
        assert(armVersionString);
        strncpy(armVersionString,(char*)&buffer[2], length);
	int dataOffset = strlen (armVersionString) + 3;		// +1 for packet size byte, +1 for packet ID, +1 for null byte

	armNumJoints = buffer[dataOffset];
	if (armJoints)
		delete[] armJoints;
	if (armNumJoints <= 0)
		return;
	armJoints = new ArmJoint[armNumJoints];
	dataOffset += 1;
	for (int ii = 0; ii < armNumJoints; ii++)
	{
		armJoints[ii].speed = buffer[dataOffset + (ii * 6)];
		armJoints[ii].home = buffer[dataOffset + (ii * 6) + 1];
		armJoints[ii].min = buffer[dataOffset + (ii * 6) + 2];
		armJoints[ii].centre = buffer[dataOffset + (ii * 6) + 3];
		armJoints[ii].max = buffer[dataOffset + (ii * 6) + 4];
		armJoints[ii].ticksPer90 = buffer[dataOffset + (ii * 6) + 5];
	}
}