rflex.cc

机器人仿真软件

  //length
  rflex_configs.m_length=
    cf->ReadFloat(section, "m_length",0.5);
  //width
  rflex_configs.m_width=
    cf->ReadFloat(section, "m_width",0.5);
  //distance conversion
  rflex_configs.odo_distance_conversion=
    cf->ReadFloat(section, "odo_distance_conversion", 0.0);
  //angle conversion
  rflex_configs.odo_angle_conversion=
    cf->ReadFloat(section, "odo_angle_conversion", 0.0);
  //default trans accel
  rflex_configs.mPsec2_trans_acceleration=
    cf->ReadFloat(section, "default_trans_acceleration",0.1);
  //default rot accel
  rflex_configs.radPsec2_rot_acceleration=
    cf->ReadFloat(section, "default_rot_acceleration",0.1);

  // absolute heading options
  rflex_configs.heading_home_address =
    cf->ReadInt(section, "rflex_heading_home_address",0);
  rflex_configs.home_on_start =
    cf->ReadInt(section, "rflex_home_on_start",0);

  // use rflex joystick for position
  rflex_configs.use_joystick |= cf->ReadInt(section, "rflex_joystick",0);
  rflex_configs.joy_pos_ratio = cf->ReadFloat(section, "rflex_joy_pos_ratio",0);
  rflex_configs.joy_ang_ratio = cf->ReadFloat(section, "rflex_joy_ang_ratio",0);

  ////////////////////////////////////////////////////////////////////////
  // Sonar-related options
  int x;

  rflex_configs.range_distance_conversion=
          cf->ReadFloat(section, "range_distance_conversion",1476);
  rflex_configs.max_num_sonars=
          cf->ReadInt(section, "max_num_sonars",64);
  rflex_configs.num_sonars=
          cf->ReadInt(section, "num_sonars",24);
  rflex_configs.sonar_age=
          cf->ReadInt(section, "sonar_age",1);
  rflex_configs.sonar_max_range=
          cf->ReadInt(section, "sonar_max_range",3000);
  rflex_configs.num_sonar_banks=
          cf->ReadInt(section, "num_sonar_banks",8);
  rflex_configs.num_sonars_possible_per_bank=
          cf->ReadInt(section, "num_sonars_possible_per_bank",16);
  rflex_configs.num_sonars_in_bank=(int *) malloc(rflex_configs.num_sonar_banks*sizeof(double));
  for(x=0;x<rflex_configs.num_sonar_banks;x++)
    rflex_configs.num_sonars_in_bank[x]=
            (int) cf->ReadTupleFloat(section, "num_sonars_in_bank",x,8);
  rflex_configs.sonar_echo_delay=
          cf->ReadInt(section, "sonar_echo_delay",3000);
  rflex_configs.sonar_ping_delay=
          cf->ReadInt(section, "sonar_ping_delay",0);
  rflex_configs.sonar_set_delay=
          cf->ReadInt(section, "sonar_set_delay", 0);
  rflex_configs.mrad_sonar_poses=(player_pose_t *) malloc(rflex_configs.num_sonars*sizeof(player_pose_t));
  for(x=0;x<rflex_configs.num_sonars;x++)
  {
    rflex_configs.mrad_sonar_poses[x].px=
            cf->ReadTupleFloat(section, "mrad_sonar_poses",3*x+1,0.0);
    rflex_configs.mrad_sonar_poses[x].py=
            cf->ReadTupleFloat(section, "mrad_sonar_poses",3*x+2,0.0);
    rflex_configs.mrad_sonar_poses[x].pa=
            cf->ReadTupleFloat(section, "mrad_sonar_poses",3*x,0.0);
  }
  rflex_configs.sonar_2nd_bank_start=cf->ReadInt(section, "sonar_2nd_bank_start", 0);
  rflex_configs.sonar_1st_bank_end=rflex_configs.sonar_2nd_bank_start>0?rflex_configs.sonar_2nd_bank_start:rflex_configs.num_sonars;
  ////////////////////////////////////////////////////////////////////////
  // IR-related options

  int pose_count=cf->ReadInt(section, "pose_count",8);
  rflex_configs.ir_base_bank=cf->ReadInt(section, "rflex_base_bank",0);
  rflex_configs.ir_bank_count=cf->ReadInt(section, "rflex_bank_count",0);
  rflex_configs.ir_min_range=cf->ReadFloat(section,"ir_min_range",0.1);
  rflex_configs.ir_max_range=cf->ReadFloat(section,"ir_max_range",0.8);
  rflex_configs.ir_count=new int[rflex_configs.ir_bank_count];
  rflex_configs.ir_a=new double[pose_count];
  rflex_configs.ir_b=new double[pose_count];
  rflex_configs.ir_poses.poses_count=pose_count;
  unsigned int RunningTotal = 0;
  for(int i=0; i < rflex_configs.ir_bank_count; ++i)
    RunningTotal += (rflex_configs.ir_count[i]=(int) cf->ReadTupleFloat(section, "rflex_banks",i,0));

  // posecount is actually unnecasary, but for consistancy will juse use it for error checking :)
  if (RunningTotal != rflex_configs.ir_poses.poses_count)
  {
    PLAYER_WARN("Error in config file, pose_count not equal to total poses in bank description\n");
    rflex_configs.ir_poses.poses_count = RunningTotal;
  }

  //  rflex_configs.ir_poses.poses=new int16_t[rflex_configs.ir_poses.pose_count];
  for(unsigned int i=0;i<rflex_configs.ir_poses.poses_count;i++)
  {
    rflex_configs.ir_poses.poses[i].px= cf->ReadTupleFloat(section, "poses",i*3,0);
    rflex_configs.ir_poses.poses[i].py= cf->ReadTupleFloat(section, "poses",i*3+1,0);
    rflex_configs.ir_poses.poses[i].pa= cf->ReadTupleFloat(section, "poses",i*3+2,0);

    // Calibration parameters for ir in form range=(a*voltage)^b
    rflex_configs.ir_a[i] = cf->ReadTupleFloat(section, "rflex_ir_calib",i*2,1);
    rflex_configs.ir_b[i] = cf->ReadTupleFloat(section, "rflex_ir_calib",i*2+1,1);
  }

  ////////////////////////////////////////////////////////////////////////
  // Bumper-related options
  rflex_configs.bumper_count = cf->ReadInt(section, "bumper_count",0);
  rflex_configs.bumper_def = new player_bumper_define_t[rflex_configs.bumper_count];
  for(x=0;x<rflex_configs.bumper_count;++x)
  {
    rflex_configs.bumper_def[x].pose.px =  (cf->ReadTupleFloat(section, "bumper_def",5*x,0)); //m
    rflex_configs.bumper_def[x].pose.py =  (cf->ReadTupleFloat(section, "bumper_def",5*x+1,0)); //m
    rflex_configs.bumper_def[x].pose.pa =  (cf->ReadTupleFloat(section, "bumper_def",5*x+2,0)); //rad
    rflex_configs.bumper_def[x].length =  (cf->ReadTupleFloat(section, "bumper_def",5*x+3,0)); //m
    rflex_configs.bumper_def[x].radius =  (cf->ReadTupleFloat(section, "bumper_def",5*x+4,0));  //m
  }
  rflex_configs.bumper_address = cf->ReadInt(section, "rflex_bumper_address",DEFAULT_RFLEX_BUMPER_ADDRESS);


  const char *bumperStyleStr = cf->ReadString(section, "rflex_bumper_style",DEFAULT_RFLEX_BUMPER_STYLE);

  if(strcmp(bumperStyleStr,RFLEX_BUMPER_STYLE_BIT) == 0)
  {
    rflex_configs.bumper_style = BUMPER_BIT;
  }
  else if(strcmp(bumperStyleStr,RFLEX_BUMPER_STYLE_ADDR) == 0)
  {
    rflex_configs.bumper_style = BUMPER_ADDR;
  }
  else
  {
    //Invalid value
    rflex_configs.bumper_style = BUMPER_ADDR;
  }

  ////////////////////////////////////////////////////////////////////////
  // Power-related options
  rflex_configs.power_offset = cf->ReadFloat(section, "rflex_power_offset",DEFAULT_RFLEX_POWER_OFFSET);

  rflex_fd = -1;

}

int RFLEX::Setup(){
  /* now spawn reading thread */
  StartThread();
  return(0);
}

int RFLEX::Shutdown()
{
  if(rflex_fd == -1)
  {
    return 0;
  }
  StopThread();
  //make sure it doesn't go anywhere
  rflex_stop_robot(rflex_fd,(int) M2ARB_ODO_CONV(rflex_configs.mPsec2_trans_acceleration));
  //kill that infernal clicking
  rflex_sonars_off(rflex_fd);
  // release the port
  rflex_close_connection(&rflex_fd);
  return 0;
}

/* start a thread that will invoke Main() */
void
RFLEX::StartThread(void)
{
  ThreadAlive = true;
  pthread_create(&driverthread, NULL, &DummyMain, this);
}

/* wait for termination of the thread */
// this is called with the lock held
void
RFLEX::StopThread(void)
{
  void* dummy;
  ThreadAlive = false;
  Unlock();

  //pthread_cancel(driverthread);
  if(pthread_join(driverthread,&dummy))
    perror("Driver::StopThread:pthread_join()");
  Lock();
}

int
RFLEX::Subscribe(player_devaddr_t id)
{
  int setupResult;

  // do the subscription
  if((setupResult = Driver::Subscribe(id)) == 0)
  {
    // also increment the appropriate subscription counter
    switch(id.interf)
    {
      case PLAYER_POSITION2D_CODE:
        this->position_subscriptions++;
        break;
      case PLAYER_SONAR_CODE:
        this->sonar_subscriptions++;
        break;
      case PLAYER_BUMPER_CODE:
        this->bumper_subscriptions++;
        break;
      case PLAYER_IR_CODE:
        this->ir_subscriptions++;
        break;
    }
  }

  return(setupResult);
}

int
RFLEX::Unsubscribe(player_devaddr_t id)
{
  int shutdownResult;

  // do the unsubscription
  if((shutdownResult = Driver::Unsubscribe(id)) == 0)
  {
    // also decrement the appropriate subscription counter
    switch(id.interf)
    {
      case PLAYER_POSITION2D_CODE:
        --this->position_subscriptions;
        assert(this->position_subscriptions >= 0);
        break;
      case PLAYER_SONAR_CODE:
        sonar_subscriptions--;
        assert(sonar_subscriptions >= 0);
        break;
      case PLAYER_BUMPER_CODE:
        --this->bumper_subscriptions;
        assert(this->bumper_subscriptions >= 0);
        break;
      case PLAYER_IR_CODE:
        --this->ir_subscriptions;
        assert(this->ir_subscriptions >= 0);
        break;
    }
  }

  return(shutdownResult);
}

void
RFLEX::Main()
{
  PLAYER_MSG1(1,"%s","Rflex Thread Started");

  //sets up connection, and sets defaults
  //configures sonar, motor acceleration, etc.
  if(initialize_robot()<0){
    PLAYER_ERROR("ERROR, no connection to RFLEX established\n");
    return;
  }
  Lock();
  reset_odometry();
  Unlock();



  static double mPsec_speedDemand=0.0, radPsec_turnRateDemand=0.0;
  bool newmotorspeed, newmotorturn;

  int i;
  int last_sonar_subscrcount = 0;
  int last_position_subscrcount = 0;
  int last_ir_subscrcount = 0;

  while(1)
  {
    int oldstate;
    int ret;
    ret = pthread_setcancelstate(PTHREAD_CANCEL_DISABLE,&oldstate);

    // we want to turn on the sonars if someone just subscribed, and turn
    // them off if the last subscriber just unsubscribed.
    if(!last_sonar_subscrcount && this->sonar_subscriptions)
    {
      Lock();
        rflex_sonars_on(rflex_fd);
        Unlock();
    }
    else if(last_sonar_subscrcount && !(this->sonar_subscriptions))
    {
      Lock();
    rflex_sonars_off(rflex_fd);
    Unlock();
    }
    last_sonar_subscrcount = this->sonar_subscriptions;

    // we want to turn on the ir if someone just subscribed, and turn
    // it off if the last subscriber just unsubscribed.
    if(!last_ir_subscrcount && this->ir_subscriptions)
    {
      Lock();
    rflex_ir_on(rflex_fd);
    Unlock();
    }
    else if(last_ir_subscrcount && !(this->ir_subscriptions))
    {
      Lock();
    rflex_ir_off(rflex_fd);
    Unlock();
    }
    last_ir_subscrcount = this->ir_subscriptions;

    // we want to reset the odometry and enable the motors if the first
    // client just subscribed to the position device, and we want to stop
    // and disable the motors if the last client unsubscribed.

    //first user logged in
    if(!last_position_subscrcount && this->position_subscriptions)
    {
      Lock();
      //set drive defaults
      rflex_motion_set_defaults(rflex_fd);

      //make sure robot doesn't go anywhere
      rflex_stop_robot(rflex_fd,(int) (M2ARB_ODO_CONV(rflex_configs.mPsec2_trans_acceleration)));

    Unlock();
    }
    //last user logged out
    else if(last_position_subscrcount && !(this->position_subscriptions))
    {
      Lock();
      //make sure robot doesn't go anywhere
      rflex_stop_robot(rflex_fd,(int) (M2ARB_ODO_CONV(rflex_configs.mPsec2_trans_acceleration)));
      // disable motor power
      rflex_brake_on(rflex_fd);
      Unlock();
    }