Пример #1
0
ssize_t getStackTraceSafe(uintptr_t* addresses, size_t maxAddresses) {
  if (maxAddresses == 0) {
    return 0;
  }
  unw_context_t context;
  if (unw_getcontext(&context) < 0) {
    return -1;
  }
  unw_cursor_t cursor;
  if (unw_init_local(&cursor, &context) < 0) {
    return -1;
  }
  if (!getFrameInfo(&cursor, *addresses)) {
    return -1;
  }
  ++addresses;
  size_t count = 1;
  for (; count != maxAddresses; ++count, ++addresses) {
    int r = unw_step(&cursor);
    if (r < 0) {
      return -1;
    }
    if (r == 0) {
      break;
    }
    if (!getFrameInfo(&cursor, *addresses)) {
      return -1;
    }
  }
  return count;
}
void RCTCHandler::execute()
{

  // read
  for(unsigned int i = 0; i < getRCTCTeamMessageDataIn().data.size(); i++)
  {
    const rctc::Message& msg = getRCTCTeamMessageDataIn().data[i];

    if (  msg.teamID == getPlayerInfo().gameData.teamNumber && 
      !(msg.playerID == getPlayerInfo().gameData.playerNumber))
    {
      TeamMessage::Data& content = getTeamMessage().data[msg.playerID];
      content.frameInfo.setTime( getFrameInfo().getTime() );
      
      //HACK: convert to protobuf message
      content.message.set_teamnumber(msg.teamID);
      content.message.set_playernumber(msg.playerID);

      content.message.set_wasstriker(msg.mabID > 0);

      if(msg.ballPosX > 0 && msg.ballPosY > 0)
      {
        content.message.mutable_ballposition()->set_x(msg.ballPosX);
        content.message.mutable_ballposition()->set_y(msg.ballPosY);
        content.message.set_timesinceballwasseen(0);
      }
      else
      {
        // very long
        content.message.set_timesinceballwasseen(10000000);
      }

    }//end if
  }//end for


  // only send data in intervals of 500ms
  if((unsigned int)getFrameInfo().getTimeSince(lastSentTimestamp) > send_interval)
  {
    // send data
    createMessage(getRCTCTeamMessageDataOut().data);
    getRCTCTeamMessageDataOut().valid = true;

    lastSentTimestamp = getFrameInfo().getTime();
  }
  else
  {
    getRCTCTeamMessageDataOut().valid = false;
  }
}//end execute
void RCTCHandler::createMessage(rctc::Message& msg)
{
  msg.teamID = getPlayerInfo().gameData.teamNumber;
  msg.playerID = getPlayerInfo().gameData.playerNumber;
  msg.goalieID = 1;
  
  if(getPlayerInfo().isPlayingStriker)
  {
    msg.mabID = getPlayerInfo().gameData.playerNumber;
  }
  else
  {
    msg.mabID = 0;
  }

  if(getFrameInfo().getTimeSince(getBallModel().frameInfoWhenBallWasSeen.getTime()) < 3000)
  {
    msg.ballPosX = (int16_t)getBallModel().position.x;
    msg.ballPosY = (int16_t)getBallModel().position.y;
  }
  else
  {
    msg.ballPosX = 0;
    msg.ballPosY = 0;
  }
}//end createMessage
void DummyActiveGoalLocator::execute() 
{
  // reset
  getLocalGoalModel().opponentGoalIsValid = false;
  getLocalGoalModel().ownGoalIsValid = false;

  getLocalGoalModel().someGoalWasSeen = getGoalPercept().getNumberOfSeenPosts() > 0; //getSensingGoalModel().someGoalWasSeen;
  
  // opp goal is in front of me
  const GoalModel::Goal& oppGoal = getSelfLocGoalModel().getOppGoal(getCompassDirection(), getFieldInfo());
  if(((oppGoal.leftPost+oppGoal.leftPost)*0.5).x > 0)
    getLocalGoalModel().opponentGoalIsValid = true;
  else
    getLocalGoalModel().ownGoalIsValid = true;


  // copy the self loc goal
  getLocalGoalModel().goal = getSelfLocGoalModel().goal;
  
  //frame Info when goal was seen not useful! New: some_goal_was seen
  if(getGoalPercept().getNumberOfSeenPosts() > 0)
  {
    getLocalGoalModel().goal.frameInfoWhenGoalLastSeen = getFrameInfo();

    if(getLocalGoalModel().opponentGoalIsValid)
    {
      getLocalGoalModel().frameWhenOpponentGoalWasSeen = getFrameInfo();
    }
    else
    {
      getLocalGoalModel().frameWhenOwnGoalWasSeen = getFrameInfo();
    }
  }

  DEBUG_REQUEST("DummyActiveGoalLocator:draw_goal_model",
    FIELD_DRAWING_CONTEXT;
    if(getLocalGoalModel().opponentGoalIsValid)
      PEN("000000", 50);
    else
      PEN("FFFFFF", 50);

    CIRCLE(getLocalGoalModel().goal.leftPost.x, getLocalGoalModel().goal.leftPost.y, 50);
    CIRCLE(getLocalGoalModel().goal.rightPost.x, getLocalGoalModel().goal.rightPost.y, 50);
    LINE(getLocalGoalModel().goal.rightPost.x, getLocalGoalModel().goal.rightPost.y, getLocalGoalModel().goal.leftPost.x, getLocalGoalModel().goal.leftPost.y);
  );
Пример #5
0
void TFDisplay::updateFrames()
{
  typedef std::vector<std::string> V_string;
  V_string frames;
  tf_->getFrameStrings( frames );

  S_FrameInfo current_frames;

  {
    V_string::iterator it = frames.begin();
    V_string::iterator end = frames.end();
    for ( ; it != end; ++it )
    {
      const std::string& frame = *it;

      if ( frame.empty() )
      {
        continue;
      }

      FrameInfo* info = getFrameInfo( frame );
      if (!info)
      {
        info = createFrame(frame);
      }
      else
      {
        updateFrame(info);
      }

      current_frames.insert( info );
    }
  }

  {
    S_FrameInfo to_delete;
    M_FrameInfo::iterator frame_it = frames_.begin();
    M_FrameInfo::iterator frame_end = frames_.end();
    for ( ; frame_it != frame_end; ++frame_it )
    {
      if ( current_frames.find( frame_it->second ) == current_frames.end() )
      {
        to_delete.insert( frame_it->second );
      }
    }

    S_FrameInfo::iterator delete_it = to_delete.begin();
    S_FrameInfo::iterator delete_end = to_delete.end();
    for ( ; delete_it != delete_end; ++delete_it )
    {
      deleteFrame( *delete_it );
    }
  }

  causeRender();
}
Пример #6
0
    BORROWED(Box*) getGlobalsDict() {
        Box* globals = getFrameInfo()->globals;
        if (!globals)
            return NULL;

        if (PyModule_Check(globals)) {
            return globals->getAttrWrapper();
        }
        return globals;
    }
Пример #7
0
void DefaultSink::onAfterGettingFrame(unsigned frame_size,
                                      unsigned truncated_bytes,
                                      struct timeval const & presentation_time,
                                      unsigned UNUSED_PARAM(duration_in_microseconds))
{
    crLogIfD(_verbose, getFrameInfo(frame_size, truncated_bytes, presentation_time));

    if (!_have_written_first_frame) {
        // If we have NAL units encoded in "sprop parameter strings",
        // prepend these to the file:

        for (auto & param : _sprop_parameter_sets) {
            unsigned int sprop_records_size = 0;

            // Returns the binary value of each 'parameter set' specified in a "sprop-parameter-sets" string
            // (in the SDP description for a H.264/RTP stream).
            //
            // The value is returned as an array (length "numSPropRecords") of "SPropRecord"s.
            // This array is dynamically allocated by this routine, and must be delete[]d by the caller.
            SPropRecord * sprop_records = parseSPropParameterSets(param.data(), sprop_records_size);
            for (unsigned int i = 0; i < sprop_records_size; ++i) {
                write(NAL_START_CODE, sizeof(NAL_START_CODE), presentation_time);
                write(sprop_records[i].sPropBytes, sprop_records[i].sPropLength, presentation_time);
            }
            delete [] sprop_records;
        }
        _have_written_first_frame = true;
    }

    if (truncated_bytes > 0) {
        auto const BUFFER_SIZE = _receive_buffer.size();
        crLogW("DefaultSink::onAfterGettingFrame() The input frame data was too large for our buffer size ({})"
               "{}bytes of trailing data was dropped!"
               "Correct this by increasing the 'bufferSize' parameter in the 'createNew()' call to at least {}",
               BUFFER_SIZE, truncated_bytes, BUFFER_SIZE + truncated_bytes);
    }

    // Write the input data to the file, with the start code in front:
    write(NAL_START_CODE, sizeof(NAL_START_CODE), presentation_time);
    write(_receive_buffer.data(), frame_size, presentation_time);

    if (isClosed()) {
        // The output file has closed.
        // Handle this the same way as if the input source had closed:
        if (fSource != nullptr) {
            fSource->stopGettingFrames();
        }
        onSourceClosure();
        return;
    }

    // Then continue, to request the next frame of data:
    continuePlaying();
}
Пример #8
0
void LEDSetter::execute()
{
  getLEDData().change = false;

  if(!getFrameRateCheckLEDRequest().ignore && (getFrameInfo().getFrameNumber() / 4) % 2 == 0)
  {
    // get all head LEDs from frame rate check
    copyMonoLEDData(getFrameRateCheckLEDRequest(), LEDData::EarRight0, LEDData::EarLeft324);
    copyMultiLEDData(getFrameRateCheckLEDRequest(), LEDData::FaceRight0, LEDData::FaceLeft315);
  }
  else
  {
    // head LEDs from behavior
    copyMonoLEDData(getBehaviorLEDRequest(), LEDData::EarRight0, LEDData::EarLeft324);
    copyMultiLEDData(getBehaviorLEDRequest(), LEDData::FaceRight0, LEDData::FaceLeft315);
  }
  // feet and chest button from GameController
  copyMultiLEDData(getGameControllerLEDRequest(), LEDData::FootLeft, LEDData::ChestButton);

} // end execute
bool SBPLCollisionModel::doesLinkExist(std::string name, std::string group_name)
{ 
  int chain, segment;
  return getFrameInfo(name, group_name, chain, segment);
}
Пример #10
0
/** This thread controls acquisition, reads image files to get the image data,
  * and does the callbacks to send it to higher layers */
void hdf5Driver::hdf5Task (void)
{
    const char *functionName = "hdf5Task";
    int status = asynSuccess;
    epicsTimeStamp startTime, endTime;
    int imageMode, currentFrame, colorMode;
    double acquirePeriod, elapsedTime, delay;

    this->lock();

    for(;;)
    {
        int acquire;
        getIntegerParam(ADAcquire, &acquire);

        if (!acquire)
        {
            this->unlock(); // Wait for semaphore unlocked

            asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
                    "%s:%s: waiting for acquire to start\n",
                    driverName, functionName);

            status = epicsEventWait(this->mStartEventId);

            this->lock();

            acquire = 1;
            setStringParam(ADStatusMessage, "Acquiring data");
            setIntegerParam(ADNumImagesCounter, 0);
        }

        // Are there datasets loaded?
        if(!mDatasetsCount)
        {
            setStringParam(ADStatusMessage, "No datasets loaded");
            goto error;
        }

        // Get acquisition parameters
        epicsTimeGetCurrent(&startTime);
        getIntegerParam(ADImageMode, &imageMode);
        getDoubleParam(ADAcquirePeriod, &acquirePeriod);
        getIntegerParam(HDF5CurrentFrame, &currentFrame);
        setIntegerParam(ADStatus, ADStatusAcquire);
        callParamCallbacks();

        // Get information to allocate NDArray
        size_t dims[2];
        NDDataType_t dataType;
        if(getFrameInfo(currentFrame, dims, &dataType))
        {
            setStringParam(ADStatusMessage, "Failed to get frame info");
            goto error;
        }

        // Allocate NDArray
        NDArray *pImage;
        if(!(pImage = pNDArrayPool->alloc(2, dims, dataType, 0, NULL)))
        {
            setStringParam(ADStatusMessage, "Failed to allocate frame");
            goto error;
        }

        // Copy data into NDArray
        if(getFrameData(currentFrame, pImage->pData))
        {
            setStringParam(ADStatusMessage, "Failed to read frame data");
            goto error;
        }

        // Set ColorMode
        colorMode = NDColorModeMono;
        pImage->pAttributeList->add("ColorMode", "Color mode", NDAttrInt32,
                &colorMode);

        // Call plugins callbacks
        int arrayCallbacks;
        getIntegerParam(NDArrayCallbacks, &arrayCallbacks);
        if (arrayCallbacks)
        {
          this->unlock();
          asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
                    "%s:%s: calling imageData callback\n",
                    driverName, functionName);
          doCallbacksGenericPointer(pImage, NDArrayData, 0);
          this->lock();
        }
        pImage->release();

        // Get the current parameters
        int lastFrame, imageCounter, numImages, numImagesCounter;
        getIntegerParam(HDF5LastFrame,      &lastFrame);
        getIntegerParam(NDArrayCounter,     &imageCounter);
        getIntegerParam(ADNumImages,        &numImages);
        getIntegerParam(ADNumImagesCounter, &numImagesCounter);

        setIntegerParam(NDArrayCounter,     ++imageCounter);
        setIntegerParam(ADNumImagesCounter, ++numImagesCounter);
        setIntegerParam(HDF5CurrentFrame,   ++currentFrame);

        // Put the frame number and time stamp into the buffer
        pImage->uniqueId = imageCounter;
        pImage->timeStamp = startTime.secPastEpoch + startTime.nsec / 1.e9;
        updateTimeStamp(&pImage->epicsTS);

        // Prepare loop if necessary
        int loop;
        getIntegerParam(HDF5Loop, &loop);

        if (loop && currentFrame > lastFrame)
        {
            getIntegerParam(HDF5FirstFrame,   &currentFrame);
            setIntegerParam(HDF5CurrentFrame, currentFrame);
        }

        // See if acquisition is done
        if (imageMode == ADImageSingle || currentFrame > lastFrame ||
            (imageMode == ADImageMultiple && numImagesCounter >= numImages))
        {
          // First do callback on ADStatus
          setStringParam(ADStatusMessage, "Waiting for acquisition");
          setIntegerParam(ADStatus, ADStatusIdle);

          acquire = 0;
          setIntegerParam(ADAcquire, acquire);

          asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
                  "%s:%s: acquisition completed\n",
                  driverName, functionName);
        }

        callParamCallbacks();

        // Delay next acquisition and check if received STOP signal
        if(acquire)
        {
            epicsTimeGetCurrent(&endTime);
            elapsedTime = epicsTimeDiffInSeconds(&endTime, &startTime);
            delay = acquirePeriod - elapsedTime;
            asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
                      "%s:%s: delay=%f\n",
                      driverName, functionName, delay);
            if(delay > 0.0)
            {
                // Set the status to waiting to indicate we are in the delay
                setIntegerParam(ADStatus, ADStatusWaiting);
                callParamCallbacks();
                this->unlock();
                status = epicsEventWaitWithTimeout(mStopEventId, delay);
                this->lock();

                if (status == epicsEventWaitOK)
                {
                    acquire = 0;
                    if (imageMode == ADImageContinuous)
                        setIntegerParam(ADStatus, ADStatusIdle);
                    else
                        setIntegerParam(ADStatus, ADStatusAborted);

                  callParamCallbacks();
                }
            }
        }
        continue;

error:
        setIntegerParam(ADAcquire, 0);
        setIntegerParam(ADStatus, ADStatusError);
        callParamCallbacks();
        continue;
    }
}
void ParticleFilterBallLocator::execute()
{
  getBallModel().reset();

  
  // update by motion model
  motionUpdate(theSampleSet, true);
  

  if(getBallPercept().ballWasSeen)
  {
    if(theSampleSet.size() > 0)
    {
      // 
      updateByBallPercept(theSampleSet);

      // 
      resampleGT07(theSampleSet, false);
    }//end if

    // replace a random particle
    if(perceptBuffer.isFull())
    {
      if(theSampleSet.size() < 20)
      {
        theSampleSet.push_back(generateNewSample());
      }
      else
      {
        int k = Math::random(theSampleSet.size());
        theSampleSet[k] = generateNewSample();
      }
    }
  }//end if


  // calculate the model
  if(!theSampleSet.empty())
  {
    Vector2<double> mean;
    Vector2<double> meanSpeed;
    double numberOfMovingSamples = 0;
    for (unsigned int i = 0; i < theSampleSet.size(); i++)
    { 
      mean += theSampleSet[i].position;

      if(theSampleSet[i].moving)
      {
        meanSpeed += theSampleSet[i].speed;
        numberOfMovingSamples++;
      }      

    }//end for

    double meanSpeedAbs = meanSpeed.abs();
    MODIFY("ParticleFilterBallLocator:meanSpeadAbs", meanSpeedAbs);
    MODIFY("ParticleFilterBallLocator:numberOfMovingSamples", numberOfMovingSamples);


    mean /= theSampleSet.size();
    if(numberOfMovingSamples > 1)
      meanSpeed /= numberOfMovingSamples;



    // set the ball model
    if(getBallPercept().ballWasSeen)
      getBallModel().setFrameInfoWhenBallWasSeen(getFrameInfo());

    getBallModel().position = mean;
    //TODO change 13.03
    //getBallModel().speed = Vector2<double>();
    getBallModel().speed = meanSpeed*100;
    getBallModel().valid = true;

    updatePreviewModel();
  }//end if
  

  DEBUG_REQUEST("ParticleFilterBallLocator:draw_ball_on_field", drawBallModel(getBallModel()); );
Пример #12
0
 AST_stmt* getCurrentStatement() {
     assert(getFrameInfo()->stmt);
     return getFrameInfo()->stmt;
 }
Пример #13
0
 FunctionMetadata* getMD() {
     FunctionMetadata* md = getFrameInfo()->md;
     assert(md);
     assert(!cf || cf->md == md);
     return md;
 }