void start(BundleContext::Ptr pContext)
{
_pTimer = new Poco::Util::Timer;
_pContext = pContext;
_pPrefs = ServiceFinder::find<PreferencesService>(pContext);
Poco::Util::AbstractConfiguration::Keys keys;
_pPrefs->configuration()->keys("simulation.sensors", keys);
for (std::vector<std::string>::const_iterator it = keys.begin(); it != keys.end(); ++it)
{
std::string baseKey = "simulation.sensors.";
baseKey += *it;
SimulatedSensor::Params params;
params.id = SimulatedSensor::SYMBOLIC_NAME;
params.id += "#";
params.id += Poco::NumberFormatter::format(_serviceRefs.size());
params.physicalQuantity = _pPrefs->configuration()->getString(baseKey + ".physicalQuantity", "");
params.physicalUnit = _pPrefs->configuration()->getString(baseKey + ".physicalUnit", "");
params.initialValue = _pPrefs->configuration()->getDouble(baseKey + ".initialValue", 0.0);
params.delta = _pPrefs->configuration()->getDouble(baseKey + ".delta", 0.0);
params.cycles = _pPrefs->configuration()->getInt(baseKey + ".cycles", 0);
params.updateRate = _pPrefs->configuration()->getDouble(baseKey + ".updateRate", 0.0);
std::string mode = _pPrefs->configuration()->getString(baseKey + ".mode", "linear");
if (mode == "linear")
params.mode = SimulatedSensor::SIM_LINEAR;
else if (mode == "random")
params.mode = SimulatedSensor::SIM_RANDOM;
try
{
createSensor(params);
}
catch (Poco::Exception& exc)
{
pContext->logger().error(Poco::format("Cannot create simulated sensor: %s", exc.displayText()));
}
}
std::string gpxPath = _pPrefs->configuration()->getString("simulation.gnss.gpxPath", "");
if (!gpxPath.empty())
{
SimulatedGNSSSensor::Params params;
params.id = SimulatedGNSSSensor::SYMBOLIC_NAME;
params.gpxPath = gpxPath;
params.loopReplay = _pPrefs->configuration()->getBool("simulation.gnss.loopReplay", true);
params.speedUp = _pPrefs->configuration()->getDouble("simulation.gnss.speedUp", 1.0);
try
{
createGNSSSensor(params);
}
catch (Poco::Exception& exc)
{
pContext->logger().error(Poco::format("Cannot create simulated GNSS sensor: %s", exc.displayText()));
}
}
}
Slinker::Slinker(Player *pl, Pim::SpriteBatchNode *b, Pim::Vec2 pos)
: Troll(b, pos)
{
body = createCircularBody(7.f, TROLLS_SL, TROLLS_SL | PLAYER | GROUND | SENSOR, 1.13f);
sensor = createSensor(body, -8.f/PTMR);
walkAnim.firstFramePos = Pim::Vec2(0.f, 14.f);
walkAnim.frameWidth = 20;
walkAnim.frameHeight = 20;
walkAnim.frameTime = 0.1f;
walkAnim.horizontalFrames = 5;
walkAnim.totalFrames = 5;
walkAnim.framesInAnimation = 4;
deathAnim.firstFramePos = Pim::Vec2(100.f, 14.f);
deathAnim.frameWidth = 20;
deathAnim.frameHeight = 20;
deathAnim.frameTime = 0.1f;
deathAnim.horizontalFrames = 7;
deathAnim.totalFrames = 7;
deathAnim.framesInAnimation = 7;
scoreValue = 50;
deathTimer = 0.f;
dead = false;
ai = new SlinkerAI(this, pl);
walkSpeed = 8.f;
health = 60.f;
timeToDie = 1.4f;
rect = walkAnim.frameIndex(0);
}
void SensorArea::initSensorArea() {
MindService *ms = MindService::getService();
// add regions
for( int k = 0; k < sensors -> getCount(); k++ ) {
MindSensor *sensor = sensors -> getSetItem( k );
createSensor( sensor );
}
ms -> addArea( this );
}
int main(void)
{
struct sockaddr_in si_other;
int s, i, slen=sizeof(si_other);
char buf[BUFLEN];
//TEST IF First RUN
int firstRun = 1;
//VARS FOR SENSOR
struct Sensor *mySensor = createSensor();
mySensor->label = CAP_NAME;
mySensor->actions[0] = "GetTemp";
char* msg;
msg = malloc(512);
msg = getInitialMessage(mySensor);
if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1)
diep("socket");
memset((char *) &si_other, 0, sizeof(si_other));
si_other.sin_family = AF_INET;
si_other.sin_port = htons(PORT);
if (inet_aton(SRV_IP, &si_other.sin_addr)==0)
{
fprintf(stderr, "inet_aton() failed\n");
exit(1);
}
if(firstRun!=0)
{
printf("FIRST RUN \n");
printf("%s", mySensor->label);
firstRun =0;
//SEND INITIAL DISVOVERY MESSAGE
if (sendto(s, msg, BUFLEN, 0,(struct sockaddr *) &si_other, slen)==-1)
{
diep("sendto()");
}
}
/*for (i=0; i<NPACK; i++)
{
printf("Sending packet %d\n", i);
sprintf(buf, "This is packet %d\n", i);
if (sendto(s, buf, BUFLEN, 0, &si_other, slen)==-1)
diep("sendto()");
}*/
close(s);
return 0;
}
void Sensor::initSensorProxyIfNeeded() {
if (m_sensorProxy)
return;
Document* document = toDocument(getExecutionContext());
if (!document || !document->frame())
return;
auto provider = SensorProviderProxy::from(document->frame());
m_sensorProxy = provider->getSensor(m_type);
if (!m_sensorProxy) {
m_sensorProxy =
provider->createSensor(m_type, createSensorReadingFactory());
}
}
void Tumbler::createPhysics()
{
body = createCircularBody(7, TROLLS_TL, TROLLS_TL | PLAYER | GROUND | SENSOR);
sensor = createSensor(body, -8.f/PTMR);
body2 = createCircularBody(7, TROLLS_TL, TROLLS_TL | PLAYER | GROUND | SENSOR);
// Joint the two bodies together. WeldJoint didn't work. WHAT THE FLYING F**K.
// Using a revolute joint instead, turning off rotation.
b2RevoluteJointDef jd;
jd.Initialize(body, body2, body->GetPosition());
jd.localAnchorB = b2Vec2(0,-0.7);
jd.collideConnected = false;
jd.lowerAngle = 0.f;
jd.upperAngle = 0.f;
world->CreateJoint(&jd);
}
void ColladaBodyLoaderImpl::setSensor(DaeNode* extNode, Link* link, Body& body)
{
DaeLink* nodeLink = static_cast<DaeLink*>(extNode);
DaeResultSensors* sensors = parser->findSensor(nodeLink->id);
if (sensors->size() <= 0) {
return;
}
// ForceSensor--------->force6d
// GyroSensor---------->no type(empty string)
// AccelerrationSensor->imu(unit sensor)
// VisionSensor-------->pin-hole-camera
for (DaeResultSensors::iterator iters = sensors->begin(); iters != sensors->end(); iters++) {
DaeSensor* sensor = *iters;
DevicePtr device = createSensor(sensor);
device->setLink(link);
device->setLocalTranslation(extNode->transform.translate + sensor->transform.translate);
device->setLocalRotation (extNode->transform.rotate.matrix() * sensor->transform.rotate.matrix());
body.addDevice(device);
}
}
int main(int argc,char *argv[]){
int i, j;
if(argc < 2 ) {
printf("USAGE: smk # of times agents runs\n");
exit(1);
}
i = atoi(argv[1]);
pthread_t sens[i];
pthread_t calc;
srand(time(NULL));
// create the conditional variable
pthread_cond_init(&self, NULL);
pthread_mutex_init(&SENSORS, NULL);
/* create locks */
if (pthread_mutex_init(&SENSORS, NULL)) {
perror("pthread_mutex_init");
exit(1);
}
if (pthread_mutex_init(&CALC, NULL)) {
perror("pthread_mutex_init");
exit(1);
}
//actually create lock on calculator so it doesn't go
// before the sensors complete
if (pthread_mutex_lock(&CALC)) {
perror("pthread_mutex_lock");
exit(1);
}
//initializes LL + contains variables to play with
Controller();
/* create sensor threads */
for(j=0;j<i;j++) {
//create node with an integer seed for the random number and an ID number
fprintf(stdout,"creating sensor node\n");
sensor_t *theSensor = createSensor(rand()%100,j);
printf("sensor has been added with an random number id of %d and an ID of %d\n", theSensor->ISRN, theSensor->ID);
//create thread to add to LL
if(pthread_create(&(sens[j]), NULL, (void *)sensor, theSensor)){
fprintf(stdout,"failing here %d\n",j);
perror("pthread_create");
exit(1);
}
fprintf(stdout,"created sensor %d\n",j);
}
puts("starting calculator");
// kickoff calculator
if(pthread_create(&(calc), NULL, (void *)calc_temp, NULL)) {
fprintf(stdout,"failing here too!");
perror("pthread_create");
exit(1);
}
// wait for calculator to finish
pthread_join(calc, NULL);
fprintf(stdout,"Calculator has finished.\n");
//printf("Sensor count: %d\n", i);
puts("All done, bye!");
// exit program now that calc is done
exit(0);
}
void AccelSensor::setup() {
SimObject::setup();
if(mInitialized == false) {
mHostBody = Physics::getPhysicsManager()->getSimBody(mReferenceBodyName->get());
if(mHostBody == 0) {
Core::log("AccelSensor: Sensor has no valid reference body. Name was ["
+ mReferenceBodyName->get() + "]! Ignoring", true);
return;
}
createSensor();
mFirstSensorValue->set(0.0);
mSecondSensorValue->set(0.0);
mThirdSensorValue->set(0.0);
mTimeStepSize = dynamic_cast<DoubleValue*>(Core::getInstance()->
getValueManager()->getValue(SimulationConstants::VALUE_TIME_STEP_SIZE));
DoubleValue *mGravitationValue = dynamic_cast<DoubleValue*>(Core::getInstance()->
getValueManager()->getValue("/Simulation/Gravitation"));
if(mTimeStepSize == 0 || mGravitationValue == 0) {
Core::log("AccelSensor: Required Event or Value could not be found.");
return;
}
mGravitation = mGravitationValue->get();
mLastAxisOneMeasurement = 0.0;
mLastAxisTwoMeasurement = 0.0;
mLastAxisThreeMeasurement = 0.0;
mInitialized = true;
}
// rotate chosen axis about the localRotation of this sensor
Quaternion localOrientation = mLocalOrientation->get();
localOrientation.normalize();
mLocalOrientation->set(localOrientation);
Quaternion localOrientationInverse = localOrientation.getInverse();
localOrientationInverse.normalize();
Quaternion xAxis(0.0,
mSensorAxisOneValue->getX(),
mSensorAxisOneValue->getY(),
mSensorAxisOneValue->getZ());
Quaternion xAxisRotated = localOrientation * xAxis * localOrientationInverse;
mSensorAxisOne.set(xAxisRotated.getX(), xAxisRotated.getY(), xAxisRotated.getZ());
mRotatedSensorAxisOne.set(xAxisRotated.getX(),
xAxisRotated.getY(),
xAxisRotated.getZ());
Quaternion yAxis(0.0,
mSensorAxisTwoValue->getX(),
mSensorAxisTwoValue->getY(),
mSensorAxisTwoValue->getZ());
Quaternion yAxisRotated = localOrientation * yAxis * localOrientationInverse;
mSensorAxisTwo.set(yAxisRotated.getX(), yAxisRotated.getY(), yAxisRotated.getZ());
mRotatedSensorAxisTwo.set(yAxisRotated.getX(),
yAxisRotated.getY(),
yAxisRotated.getZ());
Quaternion zAxis(0.0,
mSensorAxisThreeValue->getX(),
mSensorAxisThreeValue->getY(),
mSensorAxisThreeValue->getZ());
Quaternion zAxisRotated = localOrientation * zAxis * localOrientationInverse;
mSensorAxisThree.set(zAxisRotated.getX(), zAxisRotated.getY(), zAxisRotated.getZ());
mRotatedSensorAxisThree.set(zAxisRotated.getX(),
zAxisRotated.getY(),
zAxisRotated.getZ());
if(mSensorBody == 0 || mHostBody == 0) {
Core::log("AccelSensor: The sensor has no valid host or sensor-body.");
return;
}
mSensorBody->setTextureType("None");
mSensorBody->setFaceTexture(5, "AccelBoard");
mSensorGeometry->setLocalOrientation(mLocalOrientation->get());
mSensorBody->getGeometry()->setLocalPosition(mLocalPosition->get());
Quaternion localPos(0,
mLocalPosition->getX(),
mLocalPosition->getY(),
mLocalPosition->getZ());
Quaternion bodyOrientationInverse = mHostBody->getQuaternionOrientationValue()->get().getInverse();
Quaternion rotatedLocalPosQuat = mHostBody->getQuaternionOrientationValue()->get()
* localPos * bodyOrientationInverse;
Vector3D rotatedLocalPos(rotatedLocalPosQuat.getX(),
rotatedLocalPosQuat.getY(),
rotatedLocalPosQuat.getZ());
mLastPosition = mHostBody->getPositionValue()->get() + rotatedLocalPos;
if(mLowPassFilterDelayValue != 0) {
mLowPassFilterDelay = mLowPassFilterDelayValue->get();
}
mValueOneHistory.clear();
mValueTwoHistory.clear();
mValueThreeHistory.clear();
mGlobalNoiseDeviation = mGlobalNoiseDeviationValue->get();
}
/// BEGIN OF MAIN ///
int main()
{
/// VARS FOR SOCKET ///
struct sockaddr_in si_me, si_other;
int sock, i, slen=sizeof(si_other);
char buf[BUFLEN];
char sen[512];
/// END VARS FOR SOCKET ///
/// VARS FOR SHARED MEMORY ///
int shmid;
key_t key;
char *shm, *s;
key = 5678;
/// END OF VARS FOR SHARED MEMORY ///
/// RES VARS FOR SHARED MEMORY ///
int shmid2;
key_t key2;
char *shm2, *s2;
key2 = 1234;
/// RES ŖEND OF VARS FOR SHARED MEMORY ///
/// SOCKET PREP ///
if ((sock=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1)
diep("socket");
memset((char *) &si_me, 0, sizeof(si_me));
si_me.sin_family = AF_INET;
si_me.sin_port = htons(PORT);
si_me.sin_addr.s_addr = htonl(INADDR_ANY);
if (bind(sock,(struct sockaddr *) &si_me, sizeof(si_me))==-1)
diep("bind");
/// END SOCKET PREP ///
/// SHARED MEMORY PREP ///
/*
* Create the segment.
*/
if ((shmid = shmget(key, 27, IPC_CREAT | 0666)) < 0)
{
perror("shmget");
exit(1);
}
/*
* Now we attach the segment to our data space.
*/
if ((shm = shmat(shmid, NULL, 0)) == (char *) -1)
{
perror("shmat");
exit(1);
}
/*
* Now put some things into the memory for the
* other process to read.
*/
s = shm;
/// END OF SHARED MEMORY PREP ///
/// (2) SHARED MEMORY PREP ///
if ((shmid2 = shmget(key2, 27, IPC_CREAT | 0666)) < 0)
{
perror("shmget");
exit(1);
}
if ((shm2 = shmat(shmid2, NULL, 0)) == (char *) -1)
{
perror("shmat");
exit(1);
}
s2 = shm2;
///(2) END OF SHARED MEMORY PREP ///
/// MAIN LOOP ///
while(1)
{
listen(sock,5);
/// RECIVE A MESSAGE FROM SENSORS ///
if (recvfrom(sock, buf, BUFLEN, 0,(struct sockaddr *) &si_other, &slen)==-1)
{
printf("error");
diep("recvfrom()");
}
else
{
/// add port to data ///
if(isdigit(buf[0]))
{
strcpy( s2, buf);
printf("RES %s \n",s2);
}else{
strcpy( s, buf);
struct Sensor *mySensorNow = createSensor(buf);
printf("[%s] %s : %s \n",mySensorNow->label,mySensorNow->ip,mySensorNow->port);
printf("\t1/ %s\n",mySensorNow->actions[0]);
printf("\t2/ %s\n",mySensorNow->actions[1]);
//printf("\t3/ %s\n",mySensorNow->actions[0]);
}
sleep(1);
printf("Choose one method \n");
scanf( "%s" , sen );
if (sendto(sock, sen, BUFLEN, 0,(struct sockaddr *) &si_other, slen)==-1)
{
diep("sendto()");
}
else
{
printf("msg sent \n");
}
/// End add port to data ///
}
}
return 0;
}
