sip.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  
 *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
 *                      
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/*
 * $Id: sip.cc,v 1.20 2006/01/20 02:44:25 gerkey Exp $
 *
 * part of the P2OS parser.  methods for filling and parsing server
 * information packets (SIPs)
 */
#include <stdio.h>
#include <limits.h>
#include <math.h>  /* rint(3) */
#include <sys/types.h>
#include <stdlib.h> /* for abs() */
#include <unistd.h>

#include <libplayercore/error.h>

#include "sip.h"

void SIP::Fill(player_p2os_data_t* data)
{
  ///////////////////////////////////////////////////////////////
  // odometry

  // initialize position to current offset
  data->position.pos.px = this->x_offset / 1e3;
  data->position.pos.py = this->y_offset / 1e3;
  // now transform current position by rotation if there is one
  // and add to offset
  if(this->angle_offset != 0) 
  {
    double rot = DTOR(this->angle_offset);    // convert rotation to radians
    data->position.pos.px +=  ((this->xpos/1e3) * cos(rot) - 
                               (this->ypos/1e3) * sin(rot));
    data->position.pos.py +=  ((this->xpos/1e3) * sin(rot) + 
                               (this->ypos/1e3) * cos(rot));
    data->position.pos.pa = DTOR(this->angle_offset + angle);
  }
  else 
  {
    data->position.pos.px += this->xpos / 1e3;
    data->position.pos.py += this->ypos / 1e3;
    data->position.pos.pa = DTOR(this->angle);
  }
  data->position.vel.px = (((this->lvel) + (this->rvel) ) / 2) / 1e3;
  data->position.vel.py = 0.0;
  data->position.vel.pa = ((double)(this->rvel - this->lvel) /
                           (2.0/PlayerRobotParams[param_idx].DiffConvFactor));
  data->position.stall = (unsigned char)(this->lwstall || this->rwstall);

  ///////////////////////////////////////////////////////////////
  // compass
  memset(&(data->compass),0,sizeof(data->compass));
  data->compass.pos.pa = DTOR(this->compass);

  ///////////////////////////////////////////////////////////////
  // gyro
  memset(&(data->gyro),0,sizeof(data->gyro));
  data->gyro.pos.pa = DTOR(this->gyro_rate);

  ///////////////////////////////////////////////////////////////
  // sonar
  data->sonar.ranges_count = PlayerRobotParams[param_idx].SonarNum;
  for(int i=0;i<MIN(PlayerRobotParams[param_idx].SonarNum,ARRAYSIZE(sonars));i++)
    data->sonar.ranges[i] = this->sonars[i] / 1e3;

  ///////////////////////////////////////////////////////////////
  // gripper
  data->gripper.state = (unsigned char)(this->timer >> 8);
  data->gripper.beams = (unsigned char)this->digin;

  ///////////////////////////////////////////////////////////////
  // bumper
  data->bumper.bumpers_count = 10;
  int j = 0;
  for(int i=4;i>=0;i--)
    data->bumper.bumpers[j++] = 
      (unsigned char)((this->frontbumpers >> i) & 0x01);
  for(int i=4;i>=0;i--)
    data->bumper.bumpers[j++] = 
      (unsigned char)((this->rearbumpers >> i) & 0x01);

  ///////////////////////////////////////////////////////////////
  // power
  // set the bits that indicate which fields we're using
  data->power.valid = PLAYER_POWER_MASK_VOLTS | PLAYER_POWER_MASK_PERCENT;
  data->power.volts = this->battery / 1e1;
  data->power.percent = 1e2 * (data->power.volts / P2OS_NOMINAL_VOLTAGE);

  ///////////////////////////////////////////////////////////////
  // digital I/O
  data->dio.count = (unsigned char)8;
  data->dio.digin = (unsigned int)this->digin;
  
  ///////////////////////////////////////////////////////////////
  // analog I/O
  //TODO: should do this smarter, based on which analog input is selected
  data->aio.voltages_count = (unsigned char)1;
  data->aio.voltages[0] = (this->analog / 255.0) * 5.0;

  /* CMUcam blob tracking interface.  The CMUcam only supports one blob
  ** (and therefore one channel too), so everything else is zero.  All
  ** data is storde in the blobfinder packet in Network byte order.  
  ** Note: In CMUcam terminology, X is horizontal and Y is vertical, with
  ** (0,0) being TOP-LEFT (from the camera's perspective).  Also,
  ** since CMUcam doesn't have range information, but does have a
  ** confidence value, I'm passing it back as range. */
  memset(data->blobfinder.blobs,0,
         sizeof(player_blobfinder_blob_t)*PLAYER_BLOBFINDER_MAX_BLOBS);
  data->blobfinder.width = CMUCAM_IMAGE_WIDTH;
  data->blobfinder.height = CMUCAM_IMAGE_HEIGHT;

  if (blobarea > 1)	// With filtering, definition of track is 2 pixels
  {
    data->blobfinder.blobs_count = 1;
    data->blobfinder.blobs[0].color = this->blobcolor;
    data->blobfinder.blobs[0].x = this->blobmx;
    data->blobfinder.blobs[0].y = this->blobmy;
    data->blobfinder.blobs[0].left = this->blobx1;
    data->blobfinder.blobs[0].right = this->blobx2;
    data->blobfinder.blobs[0].top = this->bloby1;
    data->blobfinder.blobs[0].bottom = this->bloby2;
    data->blobfinder.blobs[0].area = this->blobarea;
    data->blobfinder.blobs[0].range = this->blobconf;
  }
  else
    data->blobfinder.blobs_count = 0;

  // Fill in arm data
  memset (data->actarray.actuators, 0, sizeof (player_actarray_actuator_t) * PLAYER_ACTARRAY_NUM_ACTUATORS);
  data->actarray.actuators_count = armNumJoints;
  for (int ii = 0; ii < armNumJoints; ii++)
  {
    data->actarray.actuators[ii].position = armJointPosRads[ii];
    data->actarray.actuators[ii].speed = 0;
    // State is complex. It can be idle, moving, or stalled (we don't have brakes so don't need to worry about the brake state).
    // Moving means have moving state from status packet
    // Idle means have not moving state from status packet and are at target position
    // Stalled means have not moving state from status packet and are not at target position
    if (armJointMoving[ii])
      data->actarray.actuators[ii].state = PLAYER_ACTARRAY_ACTSTATE_MOVING;
    else
    {
      if (armJointPos[ii] == armJointTargetPos[ii])
        data->actarray.actuators[ii].state = PLAYER_ACTARRAY_ACTSTATE_IDLE;
      else
        data->actarray.actuators[ii].state = PLAYER_ACTARRAY_ACTSTATE_STALLED;
    }
  }
}

int SIP::PositionChange( unsigned short from, unsigned short to ) 
{
  int diff1, diff2;

  /* find difference in two directions and pick shortest */
  if ( to > from ) {
    diff1 = to - from;
    diff2 = - ( from + 4096 - to );
  }
  else {
    diff1 = to - from;
    diff2 = 4096 - from + to;
  }

  if ( abs(diff1) < abs(diff2) ) 
    return(diff1);
  else
    return(diff2);

}

void SIP::Print() 
{
  int i;

  printf("lwstall:%d rwstall:%d\n", lwstall, rwstall);

  printf("Front bumpers: ");
  for(int i=0;i<5;i++) {
    printf("%d", (frontbumpers >> i) & 0x01 );
  }
  puts("");

  printf("Rear bumpers: ");
  for(int i=0;i<5;i++) {
    printf("%d", (rearbumpers >> i) & 0x01 );
  }
  puts("");

  printf("status: 0x%x analog: %d ", status, analog);
  printf("digin: ");
  for(i=0;i<8;i++) {
    printf("%d", (digin >> 7-i ) & 0x01);
  }
  printf(" digout: ");
  for(i=0;i<8;i++) {
    printf("%d", (digout >> 7-i ) & 0x01);
  }
  puts("");
  printf("battery: %d compass: %d sonarreadings: %d\n", battery, compass, sonarreadings);
  printf("xpos: %d ypos:%d ptu:%hu timer:%hu\n", xpos, ypos, ptu, timer);
  printf("angle: %d lvel: %d rvel: %d control: %d\n", angle, lvel, rvel, control);
  
  PrintSonars();
  PrintArmInfo ();
  PrintArm ();
}

void SIP::PrintSonars() 
{
  printf("Sonars: ");
  for(int i = 0; i < 16; i++){
    printf("%hu ", sonars[i]);
  } 
  puts("");
}

void SIP::PrintArm ()
{
	printf ("Arm power is %s\tArm is %sconnected\n", (armPowerOn ? "on" : "off"), (armConnected ? "" : "not "));
	printf ("Arm joint status:\n");
	for (int ii = 0; ii < 6; ii++)
		printf ("Joint %d   %s   %d\n", ii + 1, (armJointMoving[ii] ? "Moving " : "Stopped"), armJointPos[ii]);
}

void SIP::PrintArmInfo ()
{
	printf ("Arm version:\t%s\n", armVersionString);
	printf ("Arm has %d joints:\n", armNumJoints);
	printf ("  |\tSpeed\tHome\tMin\tCentre\tMax\tTicks/90\n");
	for (int ii = 0; ii < armNumJoints; ii++)
		printf ("%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);
}

void SIP::Parse( unsigned char *buffer ) 
{
  int cnt = 0, change;
  unsigned short newxpos, newypos;

  status = buffer[cnt];
  cnt += sizeof(unsigned char);
  /*
   * Remember P2OS uses little endian: 
   * for a 2 byte short (called integer on P2OS)
   * byte0 is low byte, byte1 is high byte
   * The following code is host-machine endian independant
   * Also we must or (|) bytes together instead of casting to a
   * short * since on ARM architectures short * must be even byte aligned!
   * You can get away with this on a i386 since shorts * can be
   * odd byte aligned. But on ARM, the last bit of the pointer will be ignored!
   * The or'ing will work on either arch.
   */
  newxpos = ((buffer[cnt] | (buffer[cnt+1] << 8))
	     & 0xEFFF) % 4096; /* 15 ls-bits */
  
  if (xpos!=INT_MAX) {
    change = (int) rint(PositionChange( rawxpos, newxpos ) * 
			PlayerRobotParams[param_idx].DistConvFactor);
    if (abs(change)>100)
      PLAYER_WARN1("invalid odometry change [%d]; odometry values are tainted", change);
    else
      xpos += change;
  }
  else 
    xpos = 0;
  rawxpos = newxpos;
  cnt += sizeof(short);

  newypos = ((buffer[cnt] | (buffer[cnt+1] << 8))
	     & 0xEFFF) % 4096; /* 15 ls-bits */

  if (ypos!=INT_MAX) {
    change = (int) rint(PositionChange( rawypos, newypos ) *
			PlayerRobotParams[param_idx].DistConvFactor);
    if (abs(change)>100)
      PLAYER_WARN1("invalid odometry change [%d]; odometry values are tainted", change);
    else