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);
);
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();
}
BORROWED(Box*) getGlobalsDict() {
Box* globals = getFrameInfo()->globals;
if (!globals)
return NULL;
if (PyModule_Check(globals)) {
return globals->getAttrWrapper();
}
return globals;
}
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();
}
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);
}
/** 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, ¤tFrame);
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, ¤tFrame);
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()); );
AST_stmt* getCurrentStatement() {
assert(getFrameInfo()->stmt);
return getFrameInfo()->stmt;
}
FunctionMetadata* getMD() {
FunctionMetadata* md = getFrameInfo()->md;
assert(md);
assert(!cf || cf->md == md);
return md;
}
