lasersafe.cc

机器人仿真软件

{
  assert(hdr);
  assert(data);

  if (hdr->type==PLAYER_MSGTYPE_SYNCH)
  {
    return 0;
  }

  if(Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA, PLAYER_LASER_DATA_SCAN, laser_id))
  {
    // we got laser data, we need to deal with this
    double time = hdr->timestamp;
    bool hit = false;

    Lock ();
    // Dont do anything if this is old data.
    if (time - laser_time < 0.001)
      return 0;
    laser_time = time;

    CurrentState = *reinterpret_cast<player_laser_data *> (data);

    if (history)
    {
      double accumulated = 0.0f;
      double scanAngle = CurrentState.min_angle;
      unsigned int ii = 0;
      for (ii = 0; ii < CurrentState.ranges_count; ii += step)
      {
        accumulated = 0.0f;
        for (unsigned int jj = 0; jj < historyLength - 1; jj++)
        {
          accumulated += history[jj].ranges[ii];
        }
        accumulated += CurrentState.ranges[ii];
        if (ScanInRange (accumulated / static_cast<double> (historyLength), scanAngle))
        {
          hit = true;
          break;
        }
        // The history buffer is circular, so the current data needs to go into the correct index
        history[currentHistSlot].ranges[ii] = CurrentState.ranges[ii];
        scanAngle += (CurrentState.resolution * step);
      }
      // Copy any remaining history after encountering a hit
      for (; ii < CurrentState.ranges_count; ii += step)
        history[currentHistSlot].ranges[ii] = CurrentState.ranges[ii];
      // Increment the history slot counter and wrap it
      currentHistSlot = (currentHistSlot + 1) % (historyLength - 1);
    }
    else
    {
      double scanAngle = CurrentState.min_angle;
      // If no history then just check the current data
      for (unsigned int ii = 0; ii < CurrentState.ranges_count; ii += step)
      {
        if (ScanInRange (CurrentState.ranges[ii], scanAngle))
        {
          hit = true;
          break;
        }
        scanAngle += (CurrentState.resolution * step);
      }
    }

    if (hit)
    {
      Blocked = true;
      Unlock ();
      player_position2d_cmd_vel_t NullCmd = {0};

      position->PutMsg (InQueue, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, &NullCmd, sizeof (NullCmd), NULL);
    }
    else
    {
      Blocked = false;
      Unlock ();
    }

    return 0;
  }
  // set reply to value so the reply for this message goes straight to the given client
  if (Device::MatchDeviceAddress (hdr->addr, device_addr) && hdr->type == PLAYER_MSGTYPE_REQ)
  {
    // Forward the message
    position->PutMsg (InQueue, hdr, data);
    // Store the return address for later use
    ret_queue = resp_queue;
    // Set the message filter to look for the response
    InQueue->SetFilter (position_id.host,
                        position_id.robot,
                        position_id.interf,
                        position_id.index,
                        -1,
                        hdr->subtype);
    // No response now; it will come later after we hear back from the
    // laser
    return 0;
  }

  // Forward responses (success or failure) from the position device
  if (Device::MatchDeviceAddress (hdr->addr, position_id) &&
     (hdr->type == PLAYER_MSGTYPE_RESP_ACK || hdr->type == PLAYER_MSGTYPE_RESP_NACK))
  {
    if (!gotPoseInfo && hdr->type == PLAYER_MSGTYPE_RESP_ACK && hdr->subtype == PLAYER_POSITION2D_REQ_GET_GEOM)
    {
      boxWidth = reinterpret_cast<player_position2d_geom_t*> (data)->size.sw;
	  boxWidth += boxWidth * boxSafety;     // Add safety margin to the robot's width
      boxWidth /= 2.0f;
      gotPoseInfo = true;
    }
    else
    {
      // Copy in our address and forward the response
      hdr->addr = device_addr;
      Publish (ret_queue, hdr, data);
      // Clear the filter
      InQueue->ClearFilter ();
    }
    // No response to send; we just sent it ourselves
    return 0;
  }

  // Forward data from the position device
  if (Device::MatchDeviceAddress (hdr->addr, position_id) && hdr->type == PLAYER_MSGTYPE_DATA)
  {
    // Copy in our address and forward the response
    hdr->addr = device_addr;
    Publish (ret_queue, hdr, data);
    // No response to send; we just sent it ourselves
    return 0;
  }

  if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, device_addr))
  {
    assert (hdr->size == sizeof (player_position2d_cmd_vel_t));
    bool fwdMove = reinterpret_cast<player_position2d_cmd_vel_t*> (data)->vel.px > 0 ? true : false;
    Lock ();
    if (!Blocked || (Blocked && front && !fwdMove) || (Blocked && !front && fwdMove))
    {
      Unlock ();
      position->PutMsg (InQueue, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, data, hdr->size, &hdr->timestamp);
    }
    else
      Unlock ();
    return 0;
  }

  if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_POS, device_addr))
  {
    assert (hdr->size == sizeof (player_position2d_cmd_pos_t));
    bool fwdMove = reinterpret_cast<player_position2d_cmd_pos_t*> (data)->pos.px > 0 ? true : false;
    Lock ();
    if (!Blocked || (Blocked && front && !fwdMove) || (Blocked && !front && fwdMove))
    {
      Unlock ();
      position->PutMsg (InQueue, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_POS, data, hdr->size, &hdr->timestamp);
    }
    else
      Unlock ();
    return 0;
  }

  return -1;
}


////////////////////////////////////////////////////////////////////////////////
// Set up the underlying position device.
int LaserSafe::SetupPosition ()
{
  // Subscribe to the position.
  if (Device::MatchDeviceAddress (position_id, device_addr))
  {
    PLAYER_ERROR ("attempt to subscribe to self");
    return -1;
  }
  if (!(position = deviceTable->GetDevice (position_id)))
  {
    PLAYER_ERROR ("unable to locate suitable position2d device");
    return -1;
  }
  if (position->Subscribe (InQueue) != 0)
  {
    PLAYER_ERROR ("unable to subscribe to position2d device");
    return -1;
  }

  return 0;
}

////////////////////////////////////////////////////////////////////////////////
// Shutdown the underlying position device.
int LaserSafe::ShutdownPosition ()
{
  position->Unsubscribe (InQueue);
  return 0;
}

////////////////////////////////////////////////////////////////////////////////
// Set up the bumper
int LaserSafe::SetupLaser ()
{
  if (!(laser = deviceTable->GetDevice(laser_id)))
  {
    PLAYER_ERROR ("unable to locate suitable laser device");
    return -1;
  }
  if (laser->Subscribe(InQueue) != 0)
  {
    PLAYER_ERROR ("unable to subscribe to laser device");
    return -1;
  }

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Shut down the bumper
int LaserSafe::ShutdownLaser () {
  laser->Unsubscribe (InQueue);
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Constructor
LaserSafe::LaserSafe (ConfigFile* cf, int section)
  : Driver (cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_POSITION2D_CODE)
{
  Blocked = false;
  gotPoseInfo = false;
  needPoseInfo = false;

  position = NULL;
  // Must have a position device
  if (cf->ReadDeviceAddr (&position_id, section, "requires",
                       PLAYER_POSITION2D_CODE, -1, NULL) != 0)
  {
    SetError (-1);
    return;
  }
  position_time = 0.0;

  laser = NULL;
  // Must have a laser device
  if (cf->ReadDeviceAddr (&laser_id, section, "requires",
                       PLAYER_LASER_CODE, -1, NULL) != 0)
  {
    SetError (-1);
    return;
  }
  laser_time = 0.0;
  safeDistance = cf->ReadLength (section, "safedistance", 0.4);
  step = cf->ReadInt (section, "step", 5);
  historyLength = cf->ReadInt (section, "history", 1);
  history = NULL;
  int temp = cf->ReadInt (section, "forward", 1);
  front = temp > 0 ? true : false;
  temp = cf->ReadInt (section, "boxmode", 1);
  boxMode = temp > 0 ? true : false;
  boxWidth = cf->ReadLength (section, "boxwidth", -1.0f);
  boxSafety = cf->ReadFloat (section, "boxsafety", 0.1);

  if (boxWidth < 0.0f && boxMode)
    needPoseInfo = true;  // Don't need the pose info if not in box mode or specified in config
  else
    boxWidth /= 2.0f;   // Box is always centred on laser

  return;
}