linuxjoy.cc

机器人仿真软件

  this->yaxis = cf->ReadTupleInt(section,"axes", 1, YAXIS);
  this->deadman_button = cf->ReadInt(section,"deadman", -1);
  this->xaxis_max = cf->ReadTupleInt(section, "axis_maxima", 0, AXIS_MAX);
  this->yaxis_max = cf->ReadTupleInt(section, "axis_maxima", 1, AXIS_MAX);
  this->xaxis_min = cf->ReadTupleInt(section, "axis_minima", 0, 0);
  this->yaxis_min = cf->ReadTupleInt(section, "axis_minima", 1, 0);

  // Do we talk to a position device?
  if(cf->GetTupleCount(section, "requires"))
  {
    if(cf->ReadDeviceAddr(&(this->cmd_position_addr), section, "requires", 
                          PLAYER_POSITION2D_CODE, -1, NULL) == 0)
    {
      this->max_xspeed = cf->ReadLength(section, "max_xspeed", MAX_XSPEED);
      this->max_yawspeed = cf->ReadAngle(section, "max_yawspeed", MAX_YAWSPEED);
      this->timeout = cf->ReadFloat(section, "timeout", 5.0);
    }
  }

  return;
}

////////////////////////////////////////////////////////////////////////////////
// Set up the device.  Return 0 if things go well, and -1 otherwise.
int LinuxJoystick::Setup()
{
  // Open the joystick device
  this->fd = open(this->dev, O_RDONLY);
  if (this->fd < 1)
  {
    PLAYER_ERROR2("unable to open joystick [%s]; %s",
                  this->dev, strerror(errno));
    return -1;
  }

  this->lastread.tv_sec = this->lastread.tv_usec = 0;

  // If we're asked, open the position device
  if(this->cmd_position_addr.interf)
  {
    if(!(this->position = deviceTable->GetDevice(this->cmd_position_addr)))
    {
      PLAYER_ERROR("unable to locate suitable position device");
      return(-1);
    }
    if(this->position->Subscribe(this->InQueue) != 0)
    {
      PLAYER_ERROR("unable to subscribe to position device");
      return(-1);
    }

    // Enable the motors
    player_position2d_power_config_t motorconfig;
    motorconfig.state = 1;
    Message* msg;
    if(!(msg = this->position->Request(this->InQueue,
                                       PLAYER_MSGTYPE_REQ, 
                                       PLAYER_POSITION2D_REQ_MOTOR_POWER,
                                       (void*)&motorconfig,
                                       sizeof(motorconfig),NULL,false)))
    {
      PLAYER_WARN("failed to enable motors");
    }
    else
      delete msg;

    // Stop the robot
    player_position2d_cmd_vel_t cmd;
    memset(&cmd,0,sizeof(cmd));
    this->position->PutMsg(this->InQueue,
                           PLAYER_MSGTYPE_CMD,
                           PLAYER_POSITION2D_CMD_VEL,
                           (void*)&cmd, sizeof(player_position2d_cmd_vel_t),
                           NULL);
  }
  
  this->xpos = this->ypos = 0;
  
  // Start the device thread; spawns a new thread and executes
  // LinuxJoystick::Main(), which contains the main loop for the driver.
  this->StartThread();

  return(0);
}


////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
int LinuxJoystick::Shutdown()
{
  // Stop and join the driver thread
  this->StopThread();

  if(this->cmd_position_addr.interf)
    this->position->Unsubscribe(this->InQueue);

  // Close the joystick
  close(this->fd);

  return(0);
}


////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void LinuxJoystick::Main() 
{
  // The main loop; interact with the device here
  while (true)
  {
    // test if we are supposed to cancel
    pthread_testcancel();

    // Run and process output
    this->ReadJoy();
    
    // Write outgoing data
    this->RefreshData();

    // Send new commands to position device
    if(this->cmd_position_addr.interf)
    {
      if((this->deadman_button < 0) ||
         ((this->buttons >> this->deadman_button) & 0x01))
      {
        this->PutPositionCommand();
      }
#if 0
      else
      {
        player_position2d_cmd_vel_t cmd;
        memset(&cmd,0,sizeof(cmd));
        this->position->PutMsg(this->InQueue,
                               PLAYER_MSGTYPE_CMD,
                               PLAYER_POSITION2D_CMD_VEL,
                               (void*)&cmd, sizeof(player_position2d_cmd_vel_t),
                               NULL);
      }
#endif
    }
  }
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Read the joystick
void LinuxJoystick::ReadJoy()
{
  struct pollfd fd;
  struct js_event event;
  int count;
  
  fd.fd = this->fd;
  fd.events = POLLIN | POLLHUP;
  fd.revents = 0;

  count = poll(&fd, 1, 10);
  if (count < 0)
    PLAYER_ERROR1("poll returned error [%s]", strerror(errno));
  else if(count > 0)
  {
    // get the next event from the joystick
    read(this->fd, &event, sizeof(struct js_event));

    //printf( "value % d type %u  number %u state %X \n", 
    //        event.value, event.type, event.number, this->joy_data.buttons );

    // Update buttons
    if ((event.type & ~JS_EVENT_INIT) == JS_EVENT_BUTTON)
    {
      if (event.value)
        this->buttons |= (1 << event.number);
      else
        this->buttons &= ~(1 << event.number);
    }

    // ignore the startup events
    if (event.type & JS_EVENT_INIT)
      return;

    switch( event.type )
    {
      case JS_EVENT_AXIS:
        {
          if(event.number == this->xaxis)
          {
            this->xpos = event.value;
            if(abs(this->xpos) < this->xaxis_min)
              this->xpos = 0;
            GlobalTime->GetTime(&this->lastread);
          }
          else if(event.number == this->yaxis)
          {
            this->ypos = event.value;
            if(abs(this->ypos) < this->yaxis_min)
              this->ypos = 0;
            GlobalTime->GetTime(&this->lastread);
          }
        }	  
        break;
    }
  }
      
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Send new data out
void LinuxJoystick::RefreshData()
{
  if(this->joystick_addr.interf)
  {
    memset(&(this->joy_data),0,sizeof(player_joystick_data_t));
    this->joy_data.xpos = this->xpos;
    this->joy_data.ypos = this->ypos;
    this->joy_data.xscale = this->xaxis_max;
    this->joy_data.yscale = this->yaxis_max;
    this->joy_data.buttons = this->buttons;
    this->Publish(this->joystick_addr, NULL,
                  PLAYER_MSGTYPE_DATA, PLAYER_JOYSTICK_DATA_STATE,
                  (void*)&this->joy_data, sizeof(this->joy_data), NULL);
  }

  if(this->position_addr.interf)
  {
    memset(&(this->pos_data),0,sizeof(player_position2d_data_t));
    this->pos_data.pos.px = this->xpos;
    this->pos_data.pos.py = -this->ypos;
    this->Publish(this->position_addr, NULL,
                  PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE,
                  (void*)&this->pos_data, sizeof(this->pos_data), NULL);
  }
}

// command the robot
void LinuxJoystick::PutPositionCommand()
{
  double scaled_x, scaled_y;
  double xspeed, yawspeed;
  player_position2d_cmd_vel_t cmd;
  struct timeval curr;
  double diff;

  scaled_x = this->xpos / (double) this->xaxis_max;
  scaled_y = this->ypos / (double) this->yaxis_max;

  // sanity check
  if((scaled_x > 1.0) || (scaled_x < -1.0))
  {
    PLAYER_ERROR2("X position (%d) outside of axis max (+-%d); ignoring", 
                  this->xpos, this->xaxis_max);
    return;
  }
  if((scaled_y > 1.0) || (scaled_y < -1.0))
  {
    PLAYER_ERROR2("Y position (%d) outside of axis max (+-%d); ignoring", 
                  this->ypos, this->yaxis_max);
    return;
  }

  // Note that joysticks use X for side-to-side and Y for forward-back, and
  // also that their axes are backwards with respect to intuitive driving
  // controls.
  xspeed = -scaled_y * this->max_xspeed;
  yawspeed = -scaled_x * this->max_yawspeed;

  // Make sure we've gotten a joystick fairly recently.
  GlobalTime->GetTime(&curr);
  diff = (curr.tv_sec - curr.tv_usec/1e6) -
          (this->lastread.tv_sec - this->lastread.tv_usec/1e6);
  if(this->timeout && (diff > this->timeout) && (xspeed || yawspeed))
  {
    PLAYER_WARN("Timeout on joystick; stopping robot");
    xspeed = yawspeed = 0.0;
  }

  PLAYER_MSG2(2,"sending speeds: (%f,%f)", xspeed, yawspeed);

  memset(&cmd,0,sizeof(cmd));
  cmd.vel.px = xspeed;
  cmd.vel.pa = yawspeed;
  //cmd.type=0;
  cmd.state=1;
  this->position->PutMsg(this->InQueue,
                         PLAYER_MSGTYPE_CMD,
                         PLAYER_POSITION2D_CMD_VEL,
                         (void*)&cmd, sizeof(player_position2d_cmd_vel_t),
                         NULL);
}