obot.cc

机器人仿真软件

  if(SendCommand(OBOT_SET_ACCELERATIONS,this->max_accel,this->max_accel) < 0)
  {
    PLAYER_ERROR("failed to set accelerations on setup");
    close(this->fd);
    this->fd = -1;
    return(-1);
  }

  // start the thread to talk with the robot
  StartThread();

  return(0);
}

int
Obot::Shutdown()
{
  unsigned char deinitstr[1];

  if(this->fd == -1)
    return(0);

  StopThread();

  usleep(OBOT_DELAY_US);

  deinitstr[0] = OBOT_DEINIT;
  if(WriteBuf(deinitstr,sizeof(deinitstr)) < 0)
    PLAYER_ERROR("failed to deinitialize connection to robot");

  if(close(this->fd))
    PLAYER_ERROR1("close() failed:%s",strerror(errno));
  this->fd = -1;
  puts("Botrics Obot has been shutdown");
  return(0);
}

void 
Obot::Main()
{
  player_position2d_data_t data;
  player_power_data_t charge_data;
  double lvel_mps, rvel_mps;
  int lvel, rvel;
  int ltics, rtics;
  double last_publish_time = 0.0;
  double t;

  pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);

  // push a pthread cleanup function that stops the robot
  pthread_cleanup_push(StopRobot,this);

  for(;;)
  {
    pthread_testcancel();
    
    this->sent_new_command = false;
    ProcessMessages();
    if(!this->sent_new_command)
    {
      // Which mode are we in?
      if(this->car_command_mode)
      {
        // Car-like command mode.  Re-compute angular vel based on target
        // heading
        this->ProcessCarCommand(&this->last_car_cmd);
      }
      else
      {
        // Direct velocity command mode.  Re-send last set of velocities.
        if(this->SetVelocity(this->last_final_lvel, this->last_final_rvel) < 0)
          PLAYER_ERROR("failed to set velocity");
      }
    }
    
    // Update and publish odometry info
    if(this->GetOdom(&ltics,&rtics,&lvel,&rvel) < 0)
    {
      PLAYER_ERROR("failed to get odometry");
      //pthread_exit(NULL);
    }
    else
      this->UpdateOdom(ltics,rtics);

    // Update and publish power info
    int volt;
    if(GetBatteryVoltage(&volt) < 0)
      PLAYER_WARN("failed to get voltage");
    
    GlobalTime->GetTimeDouble(&t);
    if((t - last_publish_time) > OBOT_PUBLISH_INTERVAL)
    {
      data.pos.px = this->px;
      data.pos.py = this->py;
      data.pos.pa = this->pa;

      data.vel.py = 0;
      lvel_mps = lvel * OBOT_MPS_PER_TICK;
      rvel_mps = rvel * OBOT_MPS_PER_TICK;
      data.vel.px = (lvel_mps + rvel_mps) / 2.0;
      data.vel.pa = (rvel_mps-lvel_mps) / OBOT_AXLE_LENGTH;
      data.stall = 0;
  
      //printf("publishing: %.3f %.3f %.3f\n",
             //data.pos.px,
             //data.pos.py,
             //RTOD(data.pos.pa));
      this->Publish(this->position_addr, NULL, 
                    PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, 
                    (void*)&data,sizeof(data),NULL);
      
      charge_data.valid = PLAYER_POWER_MASK_VOLTS | PLAYER_POWER_MASK_PERCENT;
      charge_data.volts = ((float)volt) / 1e1;
      charge_data.percent = 1e2 * (charge_data.volts / 
                                   OBOT_NOMINAL_VOLTAGE);
      this->Publish(this->power_addr, NULL, 
                    PLAYER_MSGTYPE_DATA,
                    PLAYER_POWER_DATA_STATE,
                    (void*)&charge_data, sizeof(player_power_data_t), NULL);
  
      last_publish_time = t;
    }

    //usleep(OBOT_DELAY_US);
  }
  pthread_cleanup_pop(0);
}

// Process car-like command, which sets an angular position target and
// translational velocity target.  The basic idea is to compute angular
// velocity so as to servo (with P-control) to target angle.  Then pass the
// two velocities to ProcessCommand() for thresholding and unit conversion.
void 
Obot::ProcessCarCommand(player_position2d_cmd_car_t * cmd)
{
  // Cache this command for later reuse
  this->last_car_cmd = *cmd;

  // Build up a cmd_vel structure to pass to ProcessCommand()
  player_position2d_cmd_vel_t vel_cmd;
  memset(&vel_cmd,0,sizeof(vel_cmd));

  // Pass through trans vel unmodified
  vel_cmd.vel.px = cmd->velocity;

  // Compute rot vel
  double da = this->angle_diff(cmd->angle, this->pa);
  if(fabs(da) < DTOR(this->car_angle_deadzone))
    vel_cmd.vel.pa = 0.0;
  else
    vel_cmd.vel.pa = this->car_angle_p * da;

  this->ProcessCommand(&vel_cmd);
}

void
Obot::ProcessCommand(player_position2d_cmd_vel_t * cmd)
{
  double rotational_term, command_lvel, command_rvel;
  int final_lvel, final_rvel;
  double xspeed, yawspeed;

  xspeed = cmd->vel.px;
  yawspeed = cmd->vel.pa;

  // Clamp velocities according to given maxima
  // TODO: test this to see if it does the right thing.  We could clamp
  //       individual wheel velocities instead.
  if(fabs(xspeed) > this->max_xspeed)
  {
    if(xspeed > 0)
      xspeed = this->max_xspeed;
    else
      xspeed = -this->max_xspeed;
  }
  if(fabs(yawspeed) > this->max_yawspeed)
  {
    if(yawspeed > 0)
      yawspeed = this->max_yawspeed;
    else
      yawspeed = -this->max_yawspeed;
  }

  // convert (tv,rv) to (lv,rv) and send to robot
  rotational_term = yawspeed * OBOT_AXLE_LENGTH / 2.0;
  command_rvel = xspeed + rotational_term;
  command_lvel = xspeed - rotational_term;

  // sanity check on per-wheel speeds
  if(fabs(command_lvel) > OBOT_MAX_WHEELSPEED)
  {
    if(command_lvel > 0)
    {
      command_lvel = OBOT_MAX_WHEELSPEED;
      command_rvel *= OBOT_MAX_WHEELSPEED/command_lvel;
    }
    else
    {
      command_lvel = - OBOT_MAX_WHEELSPEED;
      command_rvel *= -OBOT_MAX_WHEELSPEED/command_lvel;
    }
  }
  if(fabs(command_rvel) > OBOT_MAX_WHEELSPEED)
  {
    if(command_rvel > 0)
    {
      command_rvel = OBOT_MAX_WHEELSPEED;
      command_lvel *= OBOT_MAX_WHEELSPEED/command_rvel;
    }
    else
    {
      command_rvel = - OBOT_MAX_WHEELSPEED;
      command_lvel *= -OBOT_MAX_WHEELSPEED/command_rvel;
    }
  }

  final_lvel = (int)rint(command_lvel / OBOT_MPS_PER_TICK);
  final_rvel = (int)rint(command_rvel / OBOT_MPS_PER_TICK);

  // TODO: do this min threshold smarter, to preserve desired travel 
  // direction

  /* to account for our bad low-level PID motor controller */
  if(abs(final_rvel) > 0 && abs(final_rvel) < OBOT_MIN_WHEELSPEED_TICKS)
  {
    if(final_rvel > 0)
      final_rvel = OBOT_MIN_WHEELSPEED_TICKS;
    else
      final_rvel = -OBOT_MIN_WHEELSPEED_TICKS;
  }
  if(abs(final_lvel) > 0 && abs(final_lvel) < OBOT_MIN_WHEELSPEED_TICKS)
  {
    if(final_lvel > 0)
      final_lvel = OBOT_MIN_WHEELSPEED_TICKS;
    else
      final_lvel = -OBOT_MIN_WHEELSPEED_TICKS;
  }

  if((final_lvel != last_final_lvel) ||
     (final_rvel != last_final_rvel))
  {
    if(SetVelocity(final_lvel,final_rvel) < 0)
    {
      PLAYER_ERROR("failed to set velocity");
      pthread_exit(NULL);
    }
    last_final_lvel = final_lvel;
    last_final_rvel = final_rvel;
  }
}

////////////////////////////////////////////////////////////////////////////////
// Process an incoming message
int Obot::ProcessMessage(MessageQueue * resp_queue, 
                         player_msghdr * hdr, 
                         void * data)
{
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, 
                           PLAYER_POSITION2D_CMD_VEL, 
                           this->position_addr))
  {
    // Only take the first new command (should probably take the last,
    // but...)
    if(!this->sent_new_command)
    {
      assert(hdr->size == sizeof(player_position2d_cmd_vel_t));
      this->ProcessCommand((player_position2d_cmd_vel_t*)data);
      this->sent_new_command = true;
      this->car_command_mode = false;
    }
    return(0);
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, 
                                PLAYER_POSITION2D_CMD_CAR, 
                                this->position_addr))
  {
    // Only take the first new command (should probably take the last,
    // but...)
    if(!this->sent_new_command)
    {
      assert(hdr->size == sizeof(player_position2d_cmd_vel_t));
      this->ProcessCarCommand((player_position2d_cmd_car_t*)data);
      this->sent_new_command = true;
      this->car_command_mode = true;
    }
    return(0);
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, 
                                PLAYER_POSITION2D_REQ_GET_GEOM, 
                                this->position_addr))
  {
    player_position2d_geom_t  geom;
  	
    geom.pose = this->robot_pose;
    geom.size = this->robot_size;

    this->Publish(this->position_addr, resp_queue,
                  PLAYER_MSGTYPE_RESP_ACK,
                  PLAYER_POSITION2D_REQ_GET_GEOM,
                  (void*)&geom, sizeof(geom), NULL);
    return(0);
  }
  else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
                                PLAYER_POSITION2D_REQ_MOTOR_POWER, 
                                this->position_addr))
  {
    /* motor state change request 
     *   1 = enable motors
     *   0 = disable motors (default)
     */
    if(hdr->size != sizeof(player_position2d_power_config_t))
    {
      PLAYER_WARN("Arg to motor state change request wrong size; ignoring");
      return(-1);
    }
    player_position2d_power_config_t* power_config =
            (player_position2d_power_config_t*)data;
    this->ChangeMotorState(power_config->state);

    this->Publish(this->position_addr, resp_queue,
                  PLAYER_MSGTYPE_RESP_ACK, 
                  PLAYER_POSITION2D_REQ_MOTOR_POWER);
    return(0);
  }
  else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
                                PLAYER_POSITION2D_REQ_SET_ODOM,
                                this->position_addr))
  {
    if(hdr->size != sizeof(player_position2d_set_odom_req_t))
    {
      PLAYER_WARN("Arg to odometry set requests wrong size; ignoring");
      return(-1);
    }
    player_position2d_set_odom_req_t* set_odom_req =
            (player_position2d_set_odom_req_t*)data;

    // Just overwrite our current odometric pose.
    this->px = set_odom_req->pose.px;
    this->py = set_odom_req->pose.py;
    this->pa = set_odom_req->pose.pa;

    this->Publish(this->position_addr, resp_queue,
                  PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_SET_ODOM);
    return(0);
  }
  else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
                                PLAYER_POSITION2D_REQ_RESET_ODOM,
                                this->position_addr))
  {
    if(hdr->size != sizeof(player_position2d_reset_odom_config_t))
    {
      PLAYER_WARN("Arg to odometry reset requests wrong size; ignoring");
      return(-1);
    }

    // Just overwrite our current odometric pose.
    this->px = 0.0;
    this->py = 0.0;
    this->pa = 0.0;

    this->Publish(this->position_addr, resp_queue,
                  PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_RESET_ODOM);
    return(0);
  }
  else
    return -1;
}

int
Obot::ReadBuf(unsigned char* s, size_t len)
{
  int thisnumread;
  size_t numread = 0;
  int loop;
  int maxloops=10;

  loop=0;
  while(numread < len)
  {
    //printf("loop %d of %d\n", loop,maxloops);
    // apparently the underlying PIC gets overwhelmed if we read too fast
    // wait...how can that be?
    if((thisnumread = read(this->fd,s+numread,len-numread)) < 0)
    {
      if(!this->fd_blocking && errno == EAGAIN && ++loop < maxloops)
      {
        usleep(OBOT_DELAY_US);
        continue;
      }
      PLAYER_ERROR1("read() failed: %s", strerror(errno));
      return(-1);
    }
    if(thisnumread == 0)
      PLAYER_WARN("short read");
    numread += thisnumread;
  }
  /*
  printf("read: ");
  for(size_t i=0;i<numread;i++)
    printf("%d ", s[i]);
  puts("");
  */
  return(0);
}