p2os.cc

机器人仿真平台,和stage配合运行

              if(sippacket)
              {
                sippacket->x_offset = ntohl(set_odom_req.x) -
                        sippacket->xpos;
                sippacket->y_offset = ntohl(set_odom_req.y) -
                        sippacket->ypos;
                sippacket->angle_offset = ntohs(set_odom_req.theta) -
                        sippacket->angle;
              }

              if(PutReply(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, NULL, 0))
                PLAYER_ERROR("failed to PutReply");
              break;

            case PLAYER_POSITION_MOTOR_POWER_REQ:
              /* motor state change request 
               *   1 = enable motors
               *   0 = disable motors (default)
               */
              if(config_size != sizeof(player_position_power_config_t))
              {
                puts("Arg to motor state change request wrong size; ignoring");
                if(PutReply(&id, client, PLAYER_MSGTYPE_RESP_NACK, 
                            NULL, NULL, 0))
                  PLAYER_ERROR("failed to PutReply");
                break;
              }

              player_position_power_config_t power_config;
              power_config = *((player_position_power_config_t*)config);

              motorcommand[0] = ENABLE;
              motorcommand[1] = 0x3B;
              motorcommand[2] = power_config.value;
              motorcommand[3] = 0;
              motorpacket.Build(motorcommand, 4);
              SendReceive(&motorpacket);

              if(PutReply(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, NULL, 0))
                PLAYER_ERROR("failed to PutReply");
              break;

            case PLAYER_POSITION_VELOCITY_MODE_REQ:
              /* velocity control mode:
               *   0 = direct wheel velocity control (default)
               *   1 = separate translational and rotational control
               */
              if(config_size != sizeof(player_position_velocitymode_config_t))
              {
                puts("Arg to velocity control mode change request is wrong "
                     "size; ignoring");
                if(PutReply(&id, client, PLAYER_MSGTYPE_RESP_NACK, 
                            NULL, NULL, 0))
                  PLAYER_ERROR("failed to PutReply");
                break;
              }

              player_position_velocitymode_config_t velmode_config;
              velmode_config = 
                      *((player_position_velocitymode_config_t*)config);

              if(velmode_config.value)
                direct_wheel_vel_control = false;
              else
                direct_wheel_vel_control = true;

              if(PutReply(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, NULL, 0))
                PLAYER_ERROR("failed to PutReply");
              break;

            case PLAYER_POSITION_RESET_ODOM_REQ:
              /* reset position to 0,0,0: no args */
              if(config_size != sizeof(player_position_resetodom_config_t))
              {
                puts("Arg to reset position request is wrong size; ignoring");
                if(PutReply(&id, client, PLAYER_MSGTYPE_RESP_NACK, 
                            NULL, NULL, 0))
                  PLAYER_ERROR("failed to PutReply");
                break;
              }
              ResetRawPositions();

              if(PutReply(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, NULL, 0))
                PLAYER_ERROR("failed to PutReply");
              break;

            case PLAYER_POSITION_GET_GEOM_REQ:
            {
              /* Return the robot geometry. */
              if(config_size != 1)
              {
                puts("Arg get robot geom is wrong size; ignoring");
                if(PutReply(&id, client, PLAYER_MSGTYPE_RESP_NACK, 
                            NULL, NULL, 0))
                  PLAYER_ERROR("failed to PutReply");
                break;
              }

              // TODO : get values from somewhere.
              player_position_geom_t geom;
              geom.subtype = PLAYER_POSITION_GET_GEOM_REQ;
              geom.pose[0] = htons((short) (-100));
              geom.pose[1] = htons((short) (0));
              geom.pose[2] = htons((short) (0));
              geom.size[0] = htons((short) (2 * 250));
              geom.size[1] = htons((short) (2 * 225));

              if(PutReply(&id, client, PLAYER_MSGTYPE_RESP_ACK, NULL, &geom, 
                          sizeof(geom)))
                PLAYER_ERROR("failed to PutReply");
              break;
            }
            default:
              puts("Position got unknown config request");
              if(PutReply(&id, client, PLAYER_MSGTYPE_RESP_NACK,
                          NULL, NULL, 0))
                PLAYER_ERROR("failed to PutReply");
              break;
          }
          break;

        default:
          printf("RunPsosThread: got unknown config request \"%c\"\n",
                 config[0]);

          if(PutReply(&id, client, PLAYER_MSGTYPE_RESP_NACK, NULL, NULL, 0))
            PLAYER_ERROR("failed to PutReply");
          break;
      }
    }

    /* read the clients' commands from the common buffer */
    GetCommand((unsigned char*)&command, sizeof(command));

    newmotorspeed = false;
    if( speedDemand != (int) ntohl(command.position.xspeed));
    newmotorspeed = true;
    speedDemand = (int) ntohl(command.position.xspeed);

    newmotorturn = false;
    if(turnRateDemand != (int) ntohl(command.position.yawspeed));
    newmotorturn = true;
    turnRateDemand = (int) ntohl(command.position.yawspeed);

    newgrippercommand = false;
    if(gripperCmd != command.gripper.cmd || 
       gripperArg != command.gripper.arg)
    {
      newgrippercommand = true;
    }
    gripperCmd = command.gripper.cmd;
    gripperArg = command.gripper.arg;

    /* check for sound play command */
    newsoundplay = false;
    if (command.sound.index) {
      newsoundplay = true;
      soundindex = ntohs(command.sound.index);
    }
    
    /* NEXT, write commands */
    if(direct_wheel_vel_control)
    {
      // do direct wheel velocity control here
      //printf("speedDemand: %d\t turnRateDemand: %d\n",
      //speedDemand, turnRateDemand);
      rotational_term = (M_PI/180.0) * turnRateDemand /
        PlayerRobotParams[param_idx].DiffConvFactor;
      leftvel = (speedDemand - rotational_term);
      rightvel = (speedDemand + rotational_term);
      if(fabs(leftvel) > MOTOR_MAX_SPEED)
      {
        if(leftvel > 0)
        {
          leftvel = MOTOR_MAX_SPEED;
          rightvel *= MOTOR_MAX_SPEED/leftvel;
          puts("Left wheel velocity threshholded!");
        }
        else
        {
          leftvel = -MOTOR_MAX_SPEED;
          rightvel *= -MOTOR_MAX_SPEED/leftvel;
        }
      }
      if(fabs(rightvel) > MOTOR_MAX_SPEED)
      {
        if(rightvel > 0)
        {
          rightvel = MOTOR_MAX_SPEED;
          leftvel *= MOTOR_MAX_SPEED/rightvel;
          puts("Right wheel velocity threshholded!");
        }
        else
        {
          rightvel = -MOTOR_MAX_SPEED;
          leftvel *= -MOTOR_MAX_SPEED/rightvel;
        }
      }

      // left and right velocities are in 2cm/sec 
      //printf("leftvel: %d\trightvel:%d\n", 
      //(char)(leftvel/20.0), (char)(rightvel/20.0));
      motorcommand[0] = VEL2;
      motorcommand[1] = 0x3B;
      motorcommand[2] = (char)(rightvel /
                               PlayerRobotParams[param_idx].Vel2Divisor);
      motorcommand[3] = (char)(leftvel /
                               PlayerRobotParams[param_idx].Vel2Divisor);
    }
    else
    {
      // do separate trans and rot vels
      //
      // if trans vel is new, write it;
      // if just rot vel is new, write it;
      // if neither are new, write trans.
      if((newmotorspeed || !newmotorturn) && (num_loops_since_rvel < 2))
      {
        motorcommand[0] = VEL;
        if(speedDemand >= 0)
          motorcommand[1] = 0x3B;
        else
          motorcommand[1] = 0x1B;

        absspeedDemand = (unsigned short)abs(speedDemand);
        if(absspeedDemand < MOTOR_MAX_SPEED) {
          motorcommand[2] = absspeedDemand & 0x00FF;
	  motorcommand[3] = (absspeedDemand & 0xFF00) >> 8;
	}
        else
        {
          puts("Speed demand threshholded!");
          motorcommand[2] = MOTOR_MAX_SPEED & 0x00FF;
	  motorcommand[3] = (MOTOR_MAX_SPEED & 0xFF00) >> 8;
        }
      }
      else
      {
        motorcommand[0] = RVEL;
        if(turnRateDemand >= 0)
          motorcommand[1] = 0x3B;
        else
          motorcommand[1] = 0x1B;

        absturnRateDemand = (unsigned short)abs(turnRateDemand);
        if(absturnRateDemand < MOTOR_MAX_TURNRATE) {
          motorcommand[2] = absturnRateDemand & 0x00FF;
	  motorcommand[3] = (absturnRateDemand & 0xFF00) >> 8;
	}
        else
        {
          puts("Turn rate demand threshholded!");
         motorcommand[2] = MOTOR_MAX_TURNRATE & 0x00FF;
	 motorcommand[3] = (MOTOR_MAX_TURNRATE & 0xFF00) >> 8;
        }
      }
    }
    //printf("motorpacket[0]: %d\n", motorcommand[0]);
    motorpacket.Build( motorcommand, 4);
    SendReceive(&motorpacket);//,false);

    if(newgrippercommand)
    {
      //puts("sending gripper command");
      // gripper command 
      gripcommand[0] = GRIPPER;
      gripcommand[1] = 0x3B;
      gripcommand[2] = gripperCmd & 0x00FF;
      gripcommand[3] = (gripperCmd & 0xFF00) >> 8;
      grippacket.Build( gripcommand, 4);
      SendReceive(&grippacket);

      // pass extra value to gripper if needed 
      if(gripperCmd == GRIPpress || gripperCmd == LIFTcarry ) 
      {
        gripcommand[0] = GRIPPERVAL;
        gripcommand[1] = 0x3B;
        gripcommand[2] = gripperArg & 0x00FF;
	gripcommand[3] = (gripperArg & 0xFF00) >> 8;
        grippacket.Build( gripcommand, 4);
        SendReceive(&grippacket);
      }
    }

    // send sound play command down
    if (newsoundplay) {
      soundcommand[0] = SOUND;
      soundcommand[1] = 0x3B;
      soundcommand[2] = soundindex & 0x00FF;
      soundcommand[3] = (soundindex & 0xFF00) >> 8;
      soundpacket.Build(soundcommand,4);
      SendReceive(&soundpacket);
      //printf("Playing sound: %hu\n", soundindex);
      fflush(stdout);
      // now reset command to 0
      player_sound_cmd_t sound_cmd;
      sound_cmd.index = 0;
      // TODO: who should really be the client here?
      soundp->PutCommand(this,(unsigned char*)(&sound_cmd), 
                              sizeof(sound_cmd));
      soundindex = 0;
    }
      
        
  }
  pthread_exit(NULL);
}

/* send the packet, then receive and parse an SIP */
int
P2OS::SendReceive(P2OSPacket* pkt) //, bool already_have_lock)
{
  P2OSPacket packet;
  //static SIP sippacket;
  player_p2os_data_t data;
  bzero(&data,sizeof(data));

  if((psos_fd >= 0) && sippacket)
  {
    //printf("psos_fd: %d\n", psos_fd);
    //if(!already_have_lock)
      //pthread_mutex_lock(&serial_mutex);
    if(pkt)
    {
      if(!direct_wheel_vel_control)
      {
        if(pkt->packet[3] == RVEL)
          num_loops_since_rvel = 0;
        else
          num_loops_since_rvel++;
      }
      pkt->Send(psos_fd);
    }

    /* receive a packet */
    pthread_testcancel();
    if(packet.Receive(psos_fd))
    {
      puts("RunPsosThread(): Receive errored");
      // should probably just exit, although we could try to
      // reconnect...
      //tcflush(pd->psos_fd,TCIFLUSH);
      //close(pd->psos_fd);
      //pd->psos_fd = -1;
      //usleep(2*P2OS_CYCLETIME_USEC);
      //pd->Setup();
      pthread_exit(NULL);
    }

    if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB && 
       (packet.packet[3] == 0x30 || packet.packet[3] == 0x31) ||
       (packet.packet[3] == 0x32 || packet.packet[3] == 0x33) ||
       (packet.packet[3] == 0x34))
    {
      /* It is a server packet, so process it */
      sippacket->Parse( &packet.packet[3] );
      //sippacket->Print();
      sippacket->Fill(&data, timeBegan_tv );

      PutData((unsigned char*)&data, sizeof(data),0,0);
    }
    else if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB && 
            (packet.packet[3] == 0x50 || packet.packet[3] == 0x80) ||
            (packet.packet[3] == 0xB0 || packet.packet[3] == 0xC0) ||
            (packet.packet[3] == 0xD0 || packet.packet[3] == 0xE0))
    {
      /* It is a vision packet from the old Cognachrome system*/

      /* we don't understand these yet, so ignore */
    }
    else if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB && 
            (packet.packet[3] == 0x20))
    {
      //printf("got a CONFIGpac:%d\n",packet.size);
    }
    else 
    {
      puts("got unknown packet:");
      packet.PrintHex();
    }

    //pthread_mutex_unlock(&serial_mutex);
  }
  return(0);
}

void
P2OS::ResetRawPositions()
{
  P2OSPacket pkt;
  unsigned char p2oscommand[4];

  if(sippacket)
  {
    //pthread_mutex_lock(&serial_mutex);
    sippacket->rawxpos = 0;
    sippacket->rawypos = 0;
    sippacket->xpos = 0;
    sippacket->ypos = 0;
    p2oscommand[0] = SETO;
    p2oscommand[1] = 0x3B;
    pkt.Build(p2oscommand, 2);
    SendReceive(&pkt);//,true);
  }
}

/* start a thread that will invoke Main() */
void 
P2OS::StartThread()
{
  pthread_create(&thread, NULL, &DummyMain, this);
}

/* cancel (and wait for termination) of the thread */
void 
P2OS::StopThread()
{
  void* dummy;
  pthread_cancel(thread);
  if(pthread_join(thread,&dummy))
    perror("P2OS::StopThread:pthread_join()");
}