segwayrmp.cc

机器人仿真软件

    // check for config requests from the position3d interface
    if((buffer_len = GetConfig(this->position3d_id, &client, buffer, sizeof(buffer),NULL)) > 0)
    {
      // if we write to the CAN bus as a result of the config, don't write
      // a velocity command (may need to make this smarter if we get slow
      // velocity control).
      if(HandlePosition3DConfig(client,buffer,buffer_len) > 0)
        continue;
    }

    // start with the last commanded values
    xspeed = this->last_xspeed;
    yawspeed = this->last_yawspeed;
    got_command = false;

    // Check for commands from the position interface
    if (this->position_id.code)
    {
      if(GetCommand(this->position_id, (void*) &position_cmd, 
                    sizeof(position_cmd),NULL))
      {
        // zero the command buffer, so that we can timeout if a client doesn't
        // send commands for a while
        ClearCommand(this->position_id);

        // convert to host order; let MakeVelocityCommand do the rest
        xspeed = ntohl(position_cmd.xspeed);
        yawspeed = ntohl(position_cmd.yawspeed);
        motor_enabled = position_cmd.state && motor_allow_enable;
        timeout_counter=0;
        got_command = true;
      }
    }

    // Check for commands from the position3d interface
    if (this->position3d_id.code)
    {
      if(GetCommand(this->position3d_id, (void*) &position3d_cmd, 
                    sizeof(position3d_cmd),NULL))
      {
        // zero the command buffer, so that we can timeout if a client doesn't
        // send commands for a while
        ClearCommand(this->position3d_id);

        // convert to host order; let MakeVelocityCommand do the rest
        // Position3d uses milliradians/sec, so convert here to
        // degrees/sec
        xspeed = ntohl(position3d_cmd.xspeed);
        yawspeed = (int32_t) (((double) (int32_t) ntohl(position3d_cmd.yawspeed)) / 1000 * 180 / M_PI);
        motor_enabled = position3d_cmd.state && motor_allow_enable;
        timeout_counter=0;
        got_command = true;
      }
    }
    // No commands, so we may timeout soon
    if (!got_command)*/
      timeout_counter++;

    if(timeout_counter >= RMP_TIMEOUT_CYCLES)
    {
      if(curr_xspeed || curr_yawspeed)
      {
        PLAYER_WARN("timeout exceeded without new commands; stopping robot");
        curr_xspeed = 0;
        curr_yawspeed = 0;
      }
      // set it to the limit, to prevent overflow, but keep the robot
      // stopped until a new command comes in.
      timeout_counter = RMP_TIMEOUT_CYCLES;
    }

    if(!motor_enabled) 
    {
      curr_xspeed = 0;
      curr_yawspeed = 0;
    }

    // make a velocity command... could be zero
    MakeVelocityCommand(&pkt,static_cast<int> (curr_xspeed),static_cast<int> (curr_yawspeed));
    if(Write(pkt) < 0)
      PLAYER_ERROR("error on write");
  }
}

int
SegwayRMP::ProcessMessage(MessageQueue * resp_queue,
                          player_msghdr * hdr,
                          void * data)
{
  /// @todo
  /// Handle config requests
  
  // 2-D velocity command
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, 
                           PLAYER_POSITION2D_CMD_VEL, 
                           this->position_id))
  {
    player_position2d_cmd_vel_t* cmd = (player_position2d_cmd_vel_t*)data;
    this->curr_xspeed = cmd->vel.px;
    this->curr_yawspeed = cmd->vel.pa;
    this->motor_enabled = cmd->state & this->motor_allow_enable;
    this->timeout_counter = 0;
    return 0;
  }
  // 3-D velocity command
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, 
                           PLAYER_POSITION3D_CMD_SET_VEL,
                           this->position3d_id))
  {
    player_position3d_cmd_vel_t* cmd = (player_position3d_cmd_vel_t*)data;
    this->curr_xspeed = cmd->vel.px;
    this->curr_yawspeed = cmd->vel.pyaw;
    this->motor_enabled = cmd->state & this->motor_allow_enable;
    this->timeout_counter = 0;
    return 0;
  }

  if (hdr->type == PLAYER_MSGTYPE_REQ && hdr->addr.interf == position_id.interf
          && hdr->addr.index == position_id.index)
  {
    return HandlePositionConfig(resp_queue, hdr->subtype, data, hdr->size);
  }

  if (hdr->type == PLAYER_MSGTYPE_REQ && hdr->addr.interf == position3d_id.interf
          && hdr->addr.index == position3d_id.index)
  {
    return HandlePosition3DConfig(resp_queue, hdr->subtype, data, hdr->size);
  }



  return(-1);
}
  
// helper to handle config requests
// returns 1 to indicate we wrote to the CAN bus
// returns 0 to indicate we did NOT write to CAN bus
int
SegwayRMP::HandlePositionConfig(MessageQueue* client, uint32_t subtype, void* buffer, size_t len)
{
  uint16_t rmp_cmd,rmp_val;
  //player_rmp_config_t *rmp;
  CanPacket pkt;
  
  switch(subtype) 
  {
    case PLAYER_POSITION2D_REQ_MOTOR_POWER:
      // just set a flag telling us whether we should
      // act on motor commands
      // set the commands to 0... think it will automatically
      // do this for us.  
      if(((char *) buffer)[0]) 
        this->motor_allow_enable = true;
      else
        this->motor_allow_enable = false;

      printf("SEGWAYRMP: motors state: %d\n", this->motor_allow_enable);

      Publish(position_id, client, PLAYER_MSGTYPE_RESP_ACK,subtype);
      return 0;

    case PLAYER_POSITION2D_REQ_GET_GEOM:
      player_position2d_geom_t geom;
      geom.pose.px = 0;
      geom.pose.py = 0;
      geom.pose.pa = 0;
      geom.size.sw = 0.508;
      geom.size.sl = 0.610;

      Publish(position_id, client, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_GET_GEOM, &geom, sizeof(geom),NULL);
      return 0;

    case PLAYER_POSITION2D_REQ_RESET_ODOM:
      // we'll reset all the integrators

      MakeStatusCommand(&pkt, (uint16_t)RMP_CAN_CMD_RST_INT, 
                        (uint16_t)RMP_CAN_RST_ALL);
      if(Write(pkt) < 0)
      {
        Publish(position_id, client, PLAYER_MSGTYPE_RESP_NACK,PLAYER_POSITION2D_REQ_RESET_ODOM);
      }
      else
      {

        if (Write(pkt) < 0) {
          Publish(position_id, client, PLAYER_MSGTYPE_RESP_NACK,PLAYER_POSITION2D_REQ_RESET_ODOM);
        } else {
          Publish(position_id, client, PLAYER_MSGTYPE_RESP_ACK,PLAYER_POSITION2D_REQ_RESET_ODOM);
        }
      }

      odom_x = odom_y = odom_yaw = 0.0;
      firstread = true;

      // return 1 to indicate that we wrote to the CAN bus this time
      return(0);

/*    case PLAYER_POSITION_RMP_VELOCITY_SCALE:
      rmp_cmd = RMP_CAN_CMD_MAX_VEL;
      rmp = (player_rmp_config_t *)buffer;
      rmp_val = ntohs(rmp->value);

      printf("SEGWAYRMP: velocity scale %d\n", rmp_val);

      MakeStatusCommand(&pkt, rmp_cmd, rmp_val);

      if(Write(pkt) < 0)
      {
        if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL))
          PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
      }
      else
      {
        if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK,NULL))
          PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
      }
      // return 1 to indicate that we wrote to the CAN bus this time
      return(1);

    case PLAYER_POSITION_RMP_ACCEL_SCALE:
      rmp_cmd = RMP_CAN_CMD_MAX_ACCL;
      rmp = (player_rmp_config_t *)buffer;
      rmp_val = ntohs(rmp->value);

      MakeStatusCommand(&pkt, rmp_cmd, rmp_val);

      if(Write(pkt) < 0)
      {
        if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL))
          PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
      }
      else
      {
        if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK,NULL))
          PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
      }
      // return 1 to indicate that we wrote to the CAN bus this time
      return(1);

    case PLAYER_POSITION_RMP_TURN_SCALE:
      rmp_cmd = RMP_CAN_CMD_MAX_TURN;
      rmp = (player_rmp_config_t *)buffer;
      rmp_val = ntohs(rmp->value);

      MakeStatusCommand(&pkt, rmp_cmd, rmp_val);

      if(Write(pkt) < 0)
      {
        if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL))
          PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
      }
      else
      {
        if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK,NULL))
          PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
      }
      // return 1 to indicate that we wrote to the CAN bus this time
      return(1);

    case PLAYER_POSITION_RMP_GAIN_SCHEDULE:
      rmp_cmd = RMP_CAN_CMD_GAIN_SCHED;
      rmp = (player_rmp_config_t *)buffer;
      rmp_val = ntohs(rmp->value);

      MakeStatusCommand(&pkt, rmp_cmd, rmp_val);

      if(Write(pkt) < 0)
      {
        if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL))
          PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
      }
      else
      {
        if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK,NULL))
          PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
      }
      // return 1 to indicate that we wrote to the CAN bus this time
      return(1);

    case PLAYER_POSITION_RMP_CURRENT_LIMIT:
      rmp_cmd = RMP_CAN_CMD_CURR_LIMIT;
      rmp = (player_rmp_config_t *)buffer;
      rmp_val = ntohs(rmp->value);

      MakeStatusCommand(&pkt, rmp_cmd, rmp_val);

      if(Write(pkt) < 0)
      {
        if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL))
          PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
      }
      else
      {
        if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK,NULL))
          PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
      }
      // return 1 to indicate that we wrote to the CAN bus this time
      return(1);

    case PLAYER_POSITION_RMP_RST_INTEGRATORS:
      rmp_cmd = RMP_CAN_CMD_RST_INT;
      rmp = (player_rmp_config_t *)buffer;
      rmp_val = ntohs(rmp->value);

      MakeStatusCommand(&pkt, rmp_cmd, rmp_val);

      if(Write(pkt) < 0)
      {
        if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL))
          PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
      }
      else
      {
        if (Write(pkt) < 0) {
          if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL))
            PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
        } else {
          if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK,NULL))
            PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
        }
      }
      // return 1 to indicate that we wrote to the CAN bus this time
      return(1);

    case PLAYER_POSITION_RMP_SHUTDOWN:
      MakeShutdownCommand(&pkt);

      if(Write(pkt) < 0)
      {
        if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL))
          PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
      }
      else
      {
        if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK,NULL))
          PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
      }
      // return 1 to indicate that we wrote to the CAN bus this time
      return(1);
*/
    default:
      printf("segwayrmp received unknown config request %d\n", 
             subtype);
/*      if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL))
        PLAYER_ERROR("Failed to PutReply in segwayrmp\n");*/
      break;
  }

  // return -1, to indicate that we did NOT handle the message
  return(-1);
}

// helper to handle config requests
// returns 1 to indicate we wrote to the CAN bus
// returns 0 to indicate we did NOT write to CAN bus
int
SegwayRMP::HandlePosition3DConfig(MessageQueue* client, uint32_t subtype, void* buffer, size_t len)