rflex.cc

机器人仿真软件

/* register the rflex driver in the drivertable
 *
 * returns:
 */
void
RFLEX_Register(DriverTable *table)
{
  table->AddDriver("rflex", RFLEX_Init);
}

///////////////////////////////
// Message handler functions
///////////////////////////////
void PrintHeader(player_msghdr_t hdr);
int RFLEX::ProcessMessage(MessageQueue * resp_queue, player_msghdr * hdr,
                           void * data)
{
  assert(hdr);
  assert(data);

  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_SONAR_REQ_POWER,
                        sonar_id))
  {
    assert(hdr->size == sizeof(player_sonar_power_config));
    Lock();
    if(reinterpret_cast<player_sonar_power_config_t *> (data)->state==0)
      rflex_sonars_off(rflex_fd);
    else
      rflex_sonars_on(rflex_fd);
    Unlock();
    Publish(sonar_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_SONAR_REQ_POWER,
                        sonar_id_2))
  {
    assert(hdr->size == sizeof(player_sonar_power_config));
    Lock();
    if(reinterpret_cast<player_sonar_power_config_t *> (data)->state==0)
      rflex_sonars_off(rflex_fd);
    else
      rflex_sonars_on(rflex_fd);
    Unlock();
    Publish(sonar_id_2, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_SONAR_REQ_GET_GEOM,
                        sonar_id))
  {
    assert(hdr->size == 0);

    player_sonar_geom_t geom;
    Lock();
    geom.poses_count = rflex_configs.sonar_1st_bank_end;
    for (int i = 0; i < rflex_configs.sonar_1st_bank_end; i++)
    {
      geom.poses[i] = rflex_configs.mrad_sonar_poses[i];
    }
    Unlock();
    Publish(sonar_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype, &geom,sizeof(geom));
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_SONAR_REQ_GET_GEOM,
                        sonar_id_2))
  {
    assert(hdr->size == 0);

    player_sonar_geom_t geom;
    Lock();
    geom.poses_count = (rflex_configs.num_sonars - rflex_configs.sonar_2nd_bank_start);
    for (int i = 0; i < rflex_configs.num_sonars - rflex_configs.sonar_2nd_bank_start; i++)
    {
      geom.poses[i] = rflex_configs.mrad_sonar_poses[i+rflex_configs.sonar_2nd_bank_start];
    }
    Unlock();
    Publish(sonar_id_2, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype, &geom,sizeof(geom));
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_BUMPER_GET_GEOM,
                        bumper_id))
  {
    assert(hdr->size == 0);

    player_bumper_geom_t geom;
    Lock();
    geom.bumper_def_count = rflex_configs.bumper_count;
    for (int i = 0; i < rflex_configs.bumper_count; i++)
    {
      geom.bumper_def[i] = rflex_configs.bumper_def[i];
    }
    Unlock();
    Publish(bumper_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype, &geom,sizeof(geom));
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_IR_POSE, ir_id))
  {
    Lock();
    Publish(ir_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype, &rflex_configs.ir_poses,sizeof(rflex_configs.ir_poses));
    Unlock();
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_IR_POWER, ir_id))
  {
    assert(hdr->size == sizeof(player_ir_power_req_t));
    player_ir_power_req_t * req = reinterpret_cast<player_ir_power_req_t*> (data);
    Lock();
    if (req->state == 0)
      rflex_ir_off(rflex_fd);
    else
      rflex_ir_on(rflex_fd);
    Unlock();
    Publish(ir_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_SET_ODOM, position_id))
  {
    assert(hdr->size == sizeof(player_position2d_set_odom_req));

    player_position2d_set_odom_req * set_odom_req = reinterpret_cast<player_position2d_set_odom_req*> (data);
    Lock();
    set_odometry((long) (1000*set_odom_req->pose.px),(long) (1000*set_odom_req->pose.py),(short) (1000*set_odom_req->pose.pa));
    Unlock();
    Publish(position_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_MOTOR_POWER, position_id))
  {
    assert(hdr->size == sizeof(player_position2d_power_config_t));

    Lock();
    if(((player_position2d_power_config_t*)data)->state==0)
      rflex_brake_on(rflex_fd);
    else
      rflex_brake_off(rflex_fd);
    Unlock();
    Publish(position_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_VELOCITY_MODE, position_id))
  {
    assert(hdr->size == sizeof(player_position2d_velocity_mode_config_t));
    // Does nothing, needs to be implemented
    Publish(position_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_RESET_ODOM, position_id))
  {
    Lock();
    reset_odometry();
    Unlock();
    Publish(position_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM, position_id))
  {
    assert(hdr->size == 0);

    player_position2d_geom_t geom;
    geom.pose.px = 0;
    geom.pose.py = 0;
    geom.pose.pa = 0;
    Lock();
    geom.size.sl = rflex_configs.m_length;
    geom.size.sw = rflex_configs.m_width;
    Unlock();
    Publish(position_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype, &geom,sizeof(geom));
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, position_id))
  {
    assert(hdr->size == sizeof(player_position2d_cmd_vel_t));
    Lock();
    command = *reinterpret_cast<player_position2d_cmd_vel_t *> (data);
    Unlock();
 
    // reset command_type since we have a new 
    // velocity command so we can get into the 
    // velocity control section in RFLEX::Main()
    
    command_type = 0;
    
    return 0;
  }

  return -1;
}

RFLEX::RFLEX(ConfigFile* cf, int section)
        : Driver(cf,section)
{
  // zero ids, so that we'll know later which interfaces were requested
  memset(&this->position_id, 0, sizeof(player_devaddr_t));
  memset(&this->sonar_id, 0, sizeof(player_devaddr_t));
  memset(&this->sonar_id_2, 0, sizeof(player_devaddr_t));
  memset(&this->ir_id, 0, sizeof(player_devaddr_t));
  memset(&this->bumper_id, 0, sizeof(player_devaddr_t));
  memset(&this->power_id, 0, sizeof(player_devaddr_t));
  memset(&this->aio_id, 0, sizeof(player_devaddr_t));
  memset(&this->dio_id, 0, sizeof(player_devaddr_t));
  
  command_type = 0;

  this->position_subscriptions = 0;
  this->sonar_subscriptions = 0;
  this->ir_subscriptions = 0;
  this->bumper_subscriptions = 0;

  // Do we create a robot position interface?
  if(cf->ReadDeviceAddr(&(this->position_id), section, "provides",
                      PLAYER_POSITION2D_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->position_id) != 0)
    {
      this->SetError(-1);
      return;
    }
  }
  else
  {
      PLAYER_WARN("Position2d interface not created for rflex driver");
  }

  // Do we create a sonar interface?
  if(cf->ReadDeviceAddr(&(this->sonar_id), section, "provides",
                      PLAYER_SONAR_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->sonar_id) != 0)
    {
      this->SetError(-1);
      return;
    }
  }
  else
  {
      PLAYER_WARN("sonar bank 1 interface not created for rflex driver");
  }
  // Do we create a second sonar interface?
  if(cf->ReadDeviceAddr(&(this->sonar_id_2), section, "provides",
                      PLAYER_SONAR_CODE, -1, "bank2") == 0)
  {
    if(this->AddInterface(this->sonar_id_2) != 0)
    {
      this->SetError(-1);
      return;
    }
  }
  else
  {
      PLAYER_WARN("sonar bank 2 interface not created for rflex driver");
  }

  // Do we create an ir interface?
  if(cf->ReadDeviceAddr(&(this->ir_id), section, "provides",
                      PLAYER_IR_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->ir_id) != 0)
    {
      this->SetError(-1);
      return;
    }
  }
  else
  {
      PLAYER_WARN("ir interface not created for rflex driver");
  }

  // Do we create a bumper interface?
  if(cf->ReadDeviceAddr(&(this->bumper_id), section, "provides",
                      PLAYER_BUMPER_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->bumper_id) != 0)
    {
      this->SetError(-1);
      return;
    }
  }
  else
  {
      PLAYER_WARN("bumper interface not created for rflex driver");
  }

  // Do we create a power interface?
  if(cf->ReadDeviceAddr(&(this->power_id), section, "provides",
                      PLAYER_POWER_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->power_id) != 0)
    {
      this->SetError(-1);
      return;
    }
  }
  else
  {
      PLAYER_WARN("power interface not created for rflex driver");
  }

  // Do we create an aio interface?
  if(cf->ReadDeviceAddr(&(this->aio_id), section, "provides",
                      PLAYER_AIO_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->aio_id) != 0)
    {
      this->SetError(-1);
      return;
    }
  }
  else
  {
      PLAYER_WARN("aio interface not created for rflex driver");
  }

  // Do we create a dio interface?
  if(cf->ReadDeviceAddr(&(this->dio_id), section, "provides",
                      PLAYER_DIO_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->dio_id) != 0)
    {
      this->SetError(-1);
      return;
    }
  }
  else
  {
      PLAYER_WARN("dio interface not created for rflex driver");
  }

  //just sets stuff to zero
  set_config_defaults();

  // joystick override
  joy_control = 0;

  //get serial port: everyone needs it (and we dont' want them fighting)
  strncpy(rflex_configs.serial_port,
          cf->ReadString(section, "rflex_serial_port",
                         rflex_configs.serial_port),
          sizeof(rflex_configs.serial_port));

  ////////////////////////////////////////////////////////////////////////
  // Position-related options