rflex.cc

机器人仿真软件

    last_position_subscrcount = this->position_subscriptions;

  ProcessMessages();

    if(this->position_subscriptions || rflex_configs.use_joystick || rflex_configs.home_on_start)
    {
    Lock();
    newmotorspeed = false;
    newmotorturn = false;

    if (rflex_configs.home_on_start)
    {
      command.vel.pa = M_PI/18;
      newmotorturn=true;
    }

    if(mPsec_speedDemand != command.vel.px)
    {
          newmotorspeed = true;
          mPsec_speedDemand = command.vel.px;
        }
        if(radPsec_turnRateDemand != command.vel.pa)
        {
          newmotorturn = true;
          radPsec_turnRateDemand = command.vel.pa;
        }
        /* NEXT, write commands */
        // rflex has a built in failsafe mode where if no move command is recieved in a
        // certain interval the robot stops.
        // this is a good thing given teh size of the robot...
        // if network goes down or some such and the user looses control then the robot stops
        // if the robot is running in an autonomous mdoe it is easy enough to simply
        // resend the command repeatedly

        // allow rflex joystick to overide the player command
        if (joy_control > 0)
          --joy_control;
        // only set new command if type is valid and their is a new command
        else if (command_type == 0)
        {
          rflex_set_velocity(rflex_fd,(long) M2ARB_ODO_CONV(mPsec_speedDemand),(long) RAD2ARB_ODO_CONV(radPsec_turnRateDemand),(long) M2ARB_ODO_CONV(rflex_configs.mPsec2_trans_acceleration));
          command_type = 255;
        }
        Unlock();
    }
    else
    {
      Lock();
    rflex_stop_robot(rflex_fd,(long) M2ARB_ODO_CONV(rflex_configs.mPsec2_trans_acceleration));
    Unlock();
    }

    /* Get data from robot */
  static float LastYaw = 0;
    player_rflex_data_t rflex_data = {0};

  Lock();
    update_everything(&rflex_data);
    Unlock();

  if (position_id.interf != 0)
  {
    Publish(this->position_id,NULL,PLAYER_MSGTYPE_DATA,PLAYER_POSITION2D_DATA_STATE,
            (unsigned char*)&rflex_data.position,
            sizeof(player_position2d_data_t),
            NULL);
  }
  if (sonar_id.interf != 0)
  {
      Publish(this->sonar_id,NULL,PLAYER_MSGTYPE_DATA,PLAYER_SONAR_DATA_RANGES,
            (unsigned char*)&rflex_data.sonar,
            sizeof(player_sonar_data_t),
            NULL);
  }
  // Here we check if the robot has changed Yaw...
  // If it has we need to update the geometry as well
  if (rflex_data.position.pos.pa != LastYaw)
  {
    // Transmit new sonar geometry
    double NewGeom[3];

        player_sonar_geom_t geom;

    Lock();
        geom.poses_count = rflex_configs.num_sonars - rflex_configs.sonar_2nd_bank_start;
        for (i = 0; i < rflex_configs.num_sonars - rflex_configs.sonar_2nd_bank_start; i++)
        {
      SonarRotate(rad_odo_theta, rflex_configs.mrad_sonar_poses[i+rflex_configs.sonar_2nd_bank_start].px,rflex_configs.mrad_sonar_poses[i+rflex_configs.sonar_2nd_bank_start].py,rflex_configs.mrad_sonar_poses[i+rflex_configs.sonar_2nd_bank_start].pa,NewGeom,&NewGeom[1],&NewGeom[2]);
          geom.poses[i].px = NewGeom[0];
          geom.poses[i].py = NewGeom[1];
          geom.poses[i].pa = NewGeom[2];
        }
        Unlock();
        if (sonar_id_2.interf)
        Publish(this->sonar_id_2, NULL, PLAYER_MSGTYPE_DATA, PLAYER_SONAR_DATA_GEOM,
        (unsigned char*)&geom, sizeof(player_sonar_geom_t), NULL);
  }
  LastYaw = rflex_data.position.pos.pa;

  if (sonar_id_2.interf)
      Publish(this->sonar_id_2,NULL,PLAYER_MSGTYPE_DATA,PLAYER_SONAR_DATA_RANGES,
            (unsigned char*)&rflex_data.sonar2,
            sizeof(player_sonar_data_t),
            NULL);
  if (ir_id.interf)
      Publish(this->ir_id,NULL,PLAYER_MSGTYPE_DATA,PLAYER_IR_DATA_RANGES,
            (unsigned char*)&rflex_data.ir,
            sizeof(player_ir_data_t),
            NULL);
  if (bumper_id.interf)
      Publish(this->bumper_id,NULL,PLAYER_MSGTYPE_DATA,PLAYER_BUMPER_DATA_STATE,
            (unsigned char*)&rflex_data.bumper,
            sizeof(player_bumper_data_t),
            NULL);
  if (power_id.interf)
    Publish(this->power_id,NULL,PLAYER_MSGTYPE_DATA,PLAYER_POWER_DATA_STATE,
            (unsigned char*)&rflex_data.power,
            sizeof(player_power_data_t),
            NULL);
  if (aio_id.interf != 0)
  {
      Publish(this->aio_id,NULL,PLAYER_MSGTYPE_DATA,PLAYER_AIO_DATA_STATE,
            (unsigned char*)&rflex_data.aio,
            sizeof(player_aio_data_t),
            NULL);
  }
  if (dio_id.interf != 0)
  {
      Publish(this->dio_id,NULL,PLAYER_MSGTYPE_DATA,PLAYER_DIO_DATA_VALUES,
            (unsigned char*)&rflex_data.dio,
            sizeof(player_dio_data_t),
            NULL);
  }
    ret=pthread_setcancelstate(oldstate,NULL);

  Lock();
  if (!ThreadAlive)
  {
    Unlock();
    break;
  }
  Unlock();

  // release cpu somewhat so other threads can run.
  usleep(1000);
  
  }
  pthread_exit(NULL);
}

int RFLEX::initialize_robot(){
  // Neither g++ nor I can find a definition for thread_is_running - BPG
#if 0
#ifdef _REENTRANT
  if (thread_is_running)
    {
      fprintf(stderr,"Tried to connect to the robot while the command loop "
                  "thread is running.\n");
      fprintf(stderr,"This is a bug in the code, and must be fixed.\n");
      return -1;
    }
#endif
#endif

  if (rflex_open_connection(rflex_configs.serial_port, &rflex_fd) < 0)
    return -1;

  printf("Rflex initialisation called\n");
  rflex_initialize(rflex_fd, (int) M2ARB_ODO_CONV(rflex_configs.mPsec2_trans_acceleration),(int) RAD2ARB_ODO_CONV(rflex_configs.radPsec2_rot_acceleration), 0, 0);
  printf("RFlex init done\n");

  return 0;
}

void RFLEX::reset_odometry() {
  m_odo_x=0;
  m_odo_y=0;
  rad_odo_theta= 0.0;
}

void RFLEX::set_odometry(float m_x, float m_y, float rad_theta) {
  m_odo_x=m_x;
  m_odo_y=m_y;
  rad_odo_theta=rad_theta;
}

void RFLEX::update_everything(player_rflex_data_t* d)
{

  int arb_ranges[PLAYER_SONAR_MAX_SAMPLES];
  char abumper_ranges[PLAYER_BUMPER_MAX_SAMPLES];
  unsigned char air_ranges[PLAYER_IR_MAX_SAMPLES];

  // changed these variable-size array declarations to the
  // bigger-than-necessary ones above, because older versions of gcc don't
  // support variable-size arrays.
  // int arb_ranges[rflex_configs.num_sonars];
  // char abumper_ranges[rflex_configs.bumper_count];
  // unsigned char air_ranges[rflex_configs.ir_poses.pose_count];

  static int initialized = 0;

  double m_new_range_position; double rad_new_bearing_position;
  double mPsec_t_vel;
  double radPsec_r_vel;

  int arb_t_vel, arb_r_vel;
  static int arb_last_range_position;
  static int arb_last_bearing_position;
  int arb_new_range_position;
  int arb_new_bearing_position;
  double m_displacement;
  short a_num_sonars, a_num_bumpers, a_num_ir;

  int batt,brake;

  int i;

  //update status
  rflex_update_status(rflex_fd, &arb_new_range_position,
                      &arb_new_bearing_position, &arb_t_vel,
                      &arb_r_vel);
  mPsec_t_vel=ARB2M_ODO_CONV(arb_t_vel);
  radPsec_r_vel=ARB2RAD_ODO_CONV(arb_r_vel);
  m_new_range_position=ARB2M_ODO_CONV(arb_new_range_position);
  rad_new_bearing_position=ARB2RAD_ODO_CONV(arb_new_bearing_position);

  if (!initialized) {
    initialized = 1;
  } else {
    rad_odo_theta += ARB2RAD_ODO_CONV(arb_new_bearing_position - arb_last_bearing_position);
    rad_odo_theta = normalize_theta(rad_odo_theta);
    m_displacement = ARB2M_ODO_CONV(arb_new_range_position - arb_last_range_position);

    //integrate latest motion into odometry
    m_odo_x += m_displacement * cos(rad_odo_theta);
    m_odo_y += m_displacement * sin(rad_odo_theta);
    d->position.pos.px = m_odo_x;
    d->position.pos.py = m_odo_y;
    while(rad_odo_theta<0)
      rad_odo_theta+=2*M_PI;
    while(rad_odo_theta>2*M_PI)
      rad_odo_theta-=2*M_PI;
    d->position.pos.pa = rad_odo_theta;

    d->position.vel.px = mPsec_t_vel;
    d->position.vel.pa = radPsec_r_vel;
    //TODO - get better stall information (battery draw?)
  }
  d->position.stall = false;

  arb_last_range_position = arb_new_range_position;
  arb_last_bearing_position = arb_new_bearing_position;

   //note - sonar mappings are strange - look in rflex_commands.c
  if(this->sonar_subscriptions)
  {
    // TODO - currently bad sonar data is sent back to clients
    // (not enough data buffered, so sonar sent in wrong order - missing intermittent sonar values - fix this
    a_num_sonars=rflex_configs.num_sonars;

//    pthread_testcancel();
    rflex_update_sonar(rflex_fd, a_num_sonars,
           arb_ranges);
//    pthread_testcancel();
    d->sonar.ranges_count=(rflex_configs.sonar_1st_bank_end);
    for (i = 0; i < rflex_configs.sonar_1st_bank_end; i++){
      d->sonar.ranges[i] = ARB2M_RANGE_CONV(arb_ranges[i]);
    }
    d->sonar2.ranges_count=(rflex_configs.num_sonars - rflex_configs.sonar_2nd_bank_start);
    for (i = 0; i < rflex_configs.num_sonars - rflex_configs.sonar_2nd_bank_start; i++){
      d->sonar2.ranges[i] = ARB2M_RANGE_CONV(arb_ranges[rflex_configs.sonar_2nd_bank_start+i]);
    }
  }

  // if someone is subscribed to bumpers copy internal data to device
  if(this->bumper_subscriptions)
  {
    a_num_bumpers=rflex_configs.bumper_count;

//    pthread_testcancel();
    // first make sure our internal state is up to date
    rflex_update_bumpers(rflex_fd, a_num_bumpers, abumper_ranges);
 //   pthread_testcancel();

    d->bumper.bumpers_count=(a_num_bumpers);
    memcpy(d->bumper.bumpers,abumper_ranges,a_num_bumpers);
  }

  // if someone is subscribed to irs copy internal data to device
  if(this->ir_subscriptions)
  {
    a_num_ir=rflex_configs.ir_poses.poses_count;

//    pthread_testcancel();
    // first make sure our internal state is up to date
    rflex_update_ir(rflex_fd, a_num_ir, air_ranges);
//    pthread_testcancel();

    d->ir.ranges_count = (a_num_ir);
    for (int i = 0; i < a_num_ir; ++i)
    {
      d->ir.voltages[i] = air_ranges[i];
      // using power law mapping of form range = a*voltage^b
      float range = pow(air_ranges[i],rflex_configs.ir_b[i]) * rflex_configs.ir_a[i];
      // check for min and max ranges, < min = 0 > max = max
      range = range < rflex_configs.ir_min_range ? 0 : range;
      range = range > rflex_configs.ir_max_range ? rflex_configs.ir_max_range : range;
      d->ir.ranges[i] = (range);

    }
  }

  //this would get the battery,time, and brake state (if we cared)
  //update system (battery,time, and brake also request joystick data)
  rflex_update_system(rflex_fd,&batt,&brake);
  
  // set the bits for the fields we're using
  d->power.valid = PLAYER_POWER_MASK_VOLTS | PLAYER_POWER_MASK_PERCENT;
  
  d->power.volts = static_cast<float>(batt)/100.0 + rflex_configs.power_offset;
  if (d->power.volts > 24)
    d->power.percent = 100;
  else if (d->power.volts < 20)
    d->power.percent = 0;
  else
    d->power.percent = 100.0*(d->power.volts-20.0)/4.0;
}

//this is so things don't crash if we don't load a device
//(and thus it's settings)
void RFLEX::set_config_defaults(){
  memset(&rflex_configs, 0, sizeof(rflex_configs));
  strcpy(rflex_configs.serial_port,"/dev/ttyR0");
  rflex_configs.mPsec2_trans_acceleration=0.500;
  rflex_configs.radPsec2_rot_acceleration=0.500;
}