bool Actuator::setTargetTVP(double qInit,double q_target,int signMovement,double timeInit,double timeFinal,double ta, double _time) { double val; if(getTypeTrajectoryTVP()=="MaximumSpeedAcceleration") { //Acceleration phase if (_time<(timeInit+ta) && _time>=timeInit) val=qInit+signMovement*((getAcceleration()*0.5)*square(_time-timeInit)); //Velocity constant phase if (_time>=(timeInit+ta) && _time<=(timeFinal-ta)) val=qInit+signMovement*(getSpeed()*(_time-timeInit-ta*0.5)); //Deceleration phase if (_time>(timeFinal-ta) && _time<=timeFinal) val=q_target-signMovement*((getAcceleration()*0.5)*square(timeFinal-_time)); } else if(getTypeTrajectoryTVP()=="BangBang") { //Acceleration phase if (_time<(timeFinal*0.5) && _time>=timeInit) val=qInit+signMovement*((getMaxAcceleration()*0.5)*square(_time-timeInit)); //Deceleration phase if (_time>=(timeFinal*0.5) && _time<=timeFinal) val=q_target-signMovement*((getMaxAcceleration()*0.5)*square(timeFinal-_time)); } return setTarget(val); }
void Gcdc2016::nextContainer(odcore::data::Container &a_c) { if(a_c.getDataType() == opendlv::proxy::ActuationRequest::ID()){ auto actuationRequest = a_c.getData<opendlv::proxy::ActuationRequest>(); if (actuationRequest.getIsValid()) { double braking = 0.0; double acceleration = 0.0; double roadWheelAngle = actuationRequest.getSteering(); if (actuationRequest.getAcceleration() > 0) { acceleration = actuationRequest.getAcceleration(); } else { braking = actuationRequest.getAcceleration(); } m_vehicle->SetRoadVheelAngle(roadWheelAngle+0.1); m_vehicle->SetDeceleraionRequest(braking); m_vehicle->SetThrottlePedalPosition(acceleration+1); } } }
void UnstableTile::calculateUnboundedVelocity(const sf::Time& frameTime, sf::Vector2f& nextVel) const { // distinguish damping in the air and at the ground float dampingPerSec = (getVelocity().y == 0.f) ? DAMPING_GROUND : DAMPING_AIR; // don't damp when there is active acceleration if (getAcceleration().x != 0.f) dampingPerSec = 0.f; nextVel.x = (getVelocity().x + getAcceleration().x * frameTime.asSeconds()) * pow(1 - dampingPerSec, frameTime.asSeconds()); nextVel.y = getVelocity().y + getAcceleration().y * frameTime.asSeconds(); }
void MPU9150::getAccelScaled(float *ax, float *ay, float *az) { int16_t a1, a2, a3; getAcceleration(&a1, &a2, &a3); uint8_t rangeMode = getFullScaleAccelRange(); int fullScaleRange; switch (rangeMode) { case (0) : fullScaleRange = ACCEL_FCR_2; break; case (1) : fullScaleRange = ACCEL_FCR_4; break; case (2) : fullScaleRange = ACCEL_FCR_8; break; case (3) : fullScaleRange = ACCEL_FCR_16; break; default: fullScaleRange = ACCEL_FCR_2; break; } *ax = normalize(a1, fullScaleRange); *ay = normalize(a2, fullScaleRange); *az = normalize(a3, fullScaleRange); }
//! Routine to clear accelerations buffer. //! @param[in] filter filter value. inline void updateAcceleration(float filter) { for (unsigned i = 0; i < 3; ++i) m_accel_bfr[i] = getAcceleration(i) * filter; m_accel_readings = filter; }
void CurieIMUClass::readAccelerometer(int& x, int& y, int& z) { short sx, sy, sz; getAcceleration(&sx, &sy, &sz); x = sx; y = sy; z = sz; }
void CurieImuClass::readAccelerometerScaled(float &x, float &y, float &z) { int16_t sx, sy, sz; getAcceleration(&sx, &sy, &sz); x = convertRaw(sx, accel_range); y = convertRaw(sy, accel_range); z = convertRaw(sz, accel_range); }
void getMD49data (void){ getSpeed1(); getSpeed2(); readEncoderValues(); getVolts(); getCurrent1(); getCurrent2(); getError(); getMode(); getAcceleration(); i2cdata[31]=statusRegulator; i2cdata[32]=statusTimeout; }
bool MotionManager::isShakingImpl( float minShakeDeltaThreshold ) { const vec3& accel = getAcceleration(); std::unique_lock<std::mutex> lock( sMutex ); // lock after we get the acceleration so there is no deadlock bool isShaking = false; if( mPrevAcceleration != vec3( 0 ) ) { mShakeDelta = length2(accel - mPrevAcceleration); if( length2(accel - mPrevAcceleration) > (minShakeDeltaThreshold * minShakeDeltaThreshold) ) isShaking = true; } mPrevAcceleration = accel; return isShaking; }
void SSG_Planet::drawPhysicsComponents(sf::RenderTarget* t, float vecscale) const { sf::VertexArray va(sf::Lines); va.append(sf::Vertex(sf::Vector2f(x().get_d(), y().get_d()), sf::Color::White)); va.append(sf::Vertex( sf::Vector2f(x().get_d() + getVelocity().getVector().x.get_d() * vecscale * 5.f, y().get_d() + getVelocity().getVector().y.get_d() * vecscale * 5.f) , sf::Color::White)); t->draw(va); va.clear(); va.append(sf::Vertex(sf::Vector2f(x().get_d(), y().get_d()), sf::Color::Red)); va.append(sf::Vertex( sf::Vector2f(x().get_d() + getAcceleration().getVector().x.get_d() * vecscale * 100000.f, y().get_d() + getAcceleration().getVector().y.get_d() * vecscale * 100000.f) , sf::Color::Red)); t->draw(va); }
bool ABSymCurve::getDerivative(double *buff, double time, int tag) { switch (tag) { case 0: return getValues(buff, time); case 1: return getVelocity(buff, time); case 2: return getAcceleration(buff, time); default: return false; } }
U32 LLVOTree::processUpdateMessage(LLMessageSystem *mesgsys, void **user_data, U32 block_num, EObjectUpdateType update_type, LLDataPacker *dp) { // Do base class updates... U32 retval = LLViewerObject::processUpdateMessage(mesgsys, user_data, block_num, update_type, dp); if ( (getVelocity().lengthSquared() > 0.f) ||(getAcceleration().lengthSquared() > 0.f) ||(getAngularVelocity().lengthSquared() > 0.f)) { llinfos << "ACK! Moving tree!" << llendl; setVelocity(LLVector3::zero); setAcceleration(LLVector3::zero); setAngularVelocity(LLVector3::zero); } if (update_type == OUT_TERSE_IMPROVED) { // Nothing else needs to be done for the terse message. return retval; } // // Load Instance-Specific data // if (mData) { mSpecies = ((U8 *)mData)[0]; } if (!sSpeciesTable.count(mSpecies)) { if (sSpeciesTable.size()) { SpeciesMap::const_iterator it = sSpeciesTable.begin(); mSpecies = (*it).first; } } // // Load Species-Specific data // mTreeImagep = gImageList.getImage(sSpeciesTable[mSpecies]->mTextureID); if (mTreeImagep) { gGL.getTexUnit(0)->bind(mTreeImagep.get()); } mBranchLength = sSpeciesTable[mSpecies]->mBranchLength; mTrunkLength = sSpeciesTable[mSpecies]->mTrunkLength; mLeafScale = sSpeciesTable[mSpecies]->mLeafScale; mDroop = sSpeciesTable[mSpecies]->mDroop; mTwist = sSpeciesTable[mSpecies]->mTwist; mBranches = sSpeciesTable[mSpecies]->mBranches; mDepth = sSpeciesTable[mSpecies]->mDepth; mScaleStep = sSpeciesTable[mSpecies]->mScaleStep; mTrunkDepth = sSpeciesTable[mSpecies]->mTrunkDepth; mBillboardScale = sSpeciesTable[mSpecies]->mBillboardScale; mBillboardRatio = sSpeciesTable[mSpecies]->mBillboardRatio; mTrunkAspect = sSpeciesTable[mSpecies]->mTrunkAspect; mBranchAspect = sSpeciesTable[mSpecies]->mBranchAspect; // position change not caused by us, etc. make sure to rebuild. gPipeline.markRebuild(mDrawable, LLDrawable::REBUILD_ALL); return retval; }
float AccelerometerADXL335::getAccelerationZ(int _n_readings) { return getAcceleration(pin_z_, _n_readings, ZEROG_VOLTS_Z); }
void VEquipScreen::Render() { this->equippedItems.clear(); this->inventoryItems.clear(); fw().Stage_GetPrevious(this->shared_from_this())->Render(); fw().renderer->setPalette(this->pal); fw().renderer->drawFilledRect({0, 0}, fw().Display_GetSize(), Colour{0, 0, 0, 128}); // The labels/values in the stats column are used for lots of different things, so keep them // around clear them and keep them around in a vector so we don't have 5 copies of the same // "reset unused entries" code around std::vector<sp<Label>> statsLabels; std::vector<sp<Label>> statsValues; for (int i = 0; i < 9; i++) { auto labelName = UString::format("LABEL_%d", i + 1); auto label = form->FindControlTyped<Label>(labelName); if (!label) { LogError("Failed to find UI control matching \"%s\"", labelName.c_str()); } label->SetText(""); statsLabels.push_back(label); auto valueName = UString::format("VALUE_%d", i + 1); auto value = form->FindControlTyped<Label>(valueName); if (!value) { LogError("Failed to find UI control matching \"%s\"", valueName.c_str()); } value->SetText(""); statsValues.push_back(value); } auto nameLabel = form->FindControlTyped<Label>("NAME"); auto iconGraphic = form->FindControlTyped<Graphic>("SELECTED_ICON"); // If no vehicle/equipment is highlighted (mouse-over), or if we're dragging equipment around // show the currently selected vehicle stats. // // Otherwise we show the stats of the vehicle/equipment highlighted. if (highlightedEquipment) { iconGraphic->SetImage(highlightedEquipment->equipscreen_sprite); nameLabel->SetText(tr(highlightedEquipment->name)); int statsCount = 0; // All equipment has a weight statsLabels[statsCount]->SetText(tr("Weight")); statsValues[statsCount]->SetText(UString::format("%d", highlightedEquipment->weight)); statsCount++; // Draw equipment stats switch (highlightedEquipment->type) { case VEquipmentType::Type::Engine: { auto &engineType = static_cast<const VEngineType &>(*highlightedEquipment); statsLabels[statsCount]->SetText(tr("Top Speed")); statsValues[statsCount]->SetText(UString::format("%d", engineType.top_speed)); statsCount++; statsLabels[statsCount]->SetText(tr("Power")); statsValues[statsCount]->SetText(UString::format("%d", engineType.power)); break; } case VEquipmentType::Type::Weapon: { auto &weaponType = static_cast<const VWeaponType &>(*highlightedEquipment); statsLabels[statsCount]->SetText(tr("Damage")); statsValues[statsCount]->SetText(UString::format("%d", weaponType.damage)); statsCount++; statsLabels[statsCount]->SetText(tr("Range")); statsValues[statsCount]->SetText(UString::format("%d", weaponType.range)); statsCount++; statsLabels[statsCount]->SetText(tr("Accuracy")); statsValues[statsCount]->SetText(UString::format("%d", weaponType.accuracy)); statsCount++; // Only show rounds if non-zero (IE not infinite ammo) if (highlightedEquipment->max_ammo) { statsLabels[statsCount]->SetText(tr("Rounds")); statsValues[statsCount]->SetText( UString::format("%d", highlightedEquipment->max_ammo)); statsCount++; } break; } case VEquipmentType::Type::General: { auto &generalType = static_cast<const VGeneralEquipmentType &>(*highlightedEquipment); if (generalType.accuracy_modifier) { statsLabels[statsCount]->SetText(tr("Accuracy")); statsValues[statsCount]->SetText( UString::format("%d", generalType.accuracy_modifier)); statsCount++; } if (generalType.cargo_space) { statsLabels[statsCount]->SetText(tr("Cargo")); statsValues[statsCount]->SetText( UString::format("%d", generalType.cargo_space)); statsCount++; } if (generalType.passengers) { statsLabels[statsCount]->SetText(tr("Passengers")); statsValues[statsCount]->SetText(UString::format("%d", generalType.passengers)); statsCount++; } if (generalType.alien_space) { statsLabels[statsCount]->SetText(tr("Aliens Held")); statsValues[statsCount]->SetText( UString::format("%d", generalType.alien_space)); statsCount++; } if (generalType.missile_jamming) { statsLabels[statsCount]->SetText(tr("Jamming")); statsValues[statsCount]->SetText( UString::format("%d", generalType.missile_jamming)); statsCount++; } if (generalType.shielding) { statsLabels[statsCount]->SetText(tr("Shielding")); statsValues[statsCount]->SetText(UString::format("%d", generalType.shielding)); statsCount++; } if (generalType.cloaking) { statsLabels[statsCount]->SetText(tr("Cloaks Craft")); statsCount++; } if (generalType.teleporting) { statsLabels[statsCount]->SetText(tr("Teleports")); statsCount++; } break; } } } else { auto vehicle = this->highlightedVehicle; if (!vehicle) vehicle = this->selected; nameLabel->SetText(vehicle->name); // FIXME: These stats would be great to have a generic (string?) referenced list statsLabels[0]->SetText(tr("Constitution")); if (vehicle->getConstitution() == vehicle->getMaxConstitution()) { statsValues[0]->SetText(UString::format("%d", vehicle->getConstitution())); } else { statsValues[0]->SetText(UString::format("%d/%d", vehicle->getConstitution(), vehicle->getMaxConstitution())); } statsLabels[1]->SetText(tr("Armor")); statsValues[1]->SetText(UString::format("%d", vehicle->getArmor())); // FIXME: This value doesn't seem to be the same as the %age shown in the ui? statsLabels[2]->SetText(tr("Accuracy")); statsValues[2]->SetText(UString::format("%d", vehicle->getAccuracy())); statsLabels[3]->SetText(tr("Top Speed")); statsValues[3]->SetText(UString::format("%d", vehicle->getTopSpeed())); statsLabels[4]->SetText(tr("Acceleration")); statsValues[4]->SetText(UString::format("%d", vehicle->getAcceleration())); statsLabels[5]->SetText(tr("Weight")); statsValues[5]->SetText(UString::format("%d", vehicle->getWeight())); statsLabels[6]->SetText(tr("Fuel")); statsValues[6]->SetText(UString::format("%d", vehicle->getFuel())); statsLabels[7]->SetText(tr("Passengers")); statsValues[7]->SetText( UString::format("%d/%d", vehicle->getPassengers(), vehicle->getMaxPassengers())); statsLabels[8]->SetText(tr("Cargo")); statsValues[8]->SetText( UString::format("%d/%d", vehicle->getCargo(), vehicle->getMaxCargo())); iconGraphic->SetImage(vehicle->type.equip_icon_small); } // Now draw the form, the actual equipment is then drawn on top form->Render(); auto paperDollControl = form->FindControlTyped<Graphic>("PAPER_DOLL"); Vec2<int> equipOffset = paperDollControl->Location + form->Location; // Draw the equipment grid { for (auto &slot : selected->type.equipment_layout_slots) { Vec2<int> p00 = (slot.bounds.p0 * EQUIP_GRID_SLOT_SIZE) + equipOffset; Vec2<int> p11 = (slot.bounds.p1 * EQUIP_GRID_SLOT_SIZE) + equipOffset; Vec2<int> p01 = {p00.x, p11.y}; Vec2<int> p10 = {p11.x, p00.y}; if (slot.type == selectionType) { // Scale the sin curve from (-1, 1) to (0, 1) float glowFactor = (sin(this->glowCounter) + 1.0f) / 2.0f; Colour equipColour; switch (selectionType) { case VEquipmentType::Type::Engine: equipColour = EQUIP_GRID_COLOUR_ENGINE; break; case VEquipmentType::Type::Weapon: equipColour = EQUIP_GRID_COLOUR_WEAPON; break; case VEquipmentType::Type::General: equipColour = EQUIP_GRID_COLOUR_GENERAL; break; } Colour selectedColour; selectedColour.r = mix(equipColour.r, EQUIP_GRID_COLOUR.r, glowFactor); selectedColour.g = mix(equipColour.g, EQUIP_GRID_COLOUR.g, glowFactor); selectedColour.b = mix(equipColour.b, EQUIP_GRID_COLOUR.b, glowFactor); selectedColour.a = 255; fw().renderer->drawLine(p00, p01, selectedColour, 2); fw().renderer->drawLine(p01, p11, selectedColour, 2); fw().renderer->drawLine(p11, p10, selectedColour, 2); fw().renderer->drawLine(p10, p00, selectedColour, 2); } else { fw().renderer->drawLine(p00, p01, EQUIP_GRID_COLOUR, 2); fw().renderer->drawLine(p01, p11, EQUIP_GRID_COLOUR, 2); fw().renderer->drawLine(p11, p10, EQUIP_GRID_COLOUR, 2); fw().renderer->drawLine(p10, p00, EQUIP_GRID_COLOUR, 2); } } } // Draw the equipped stuff for (auto &e : selected->equipment) { auto pos = e->equippedPosition; VehicleType::AlignmentX alignX = VehicleType::AlignmentX::Left; VehicleType::AlignmentY alignY = VehicleType::AlignmentY::Top; Rect<int> slotBounds; bool slotFound = false; for (auto &slot : this->selected->type.equipment_layout_slots) { if (slot.bounds.p0 == pos) { alignX = slot.align_x; alignY = slot.align_y; slotBounds = slot.bounds; slotFound = true; break; } } if (!slotFound) { LogError("No matching slot for equipment at {%d,%d}", pos.x, pos.y); } if (pos.x >= EQUIP_GRID_SLOTS.x || pos.y >= EQUIP_GRID_SLOTS.y) { LogError("Equipment at {%d,%d} outside grid", pos.x, pos.y); } pos *= EQUIP_GRID_SLOT_SIZE; pos += equipOffset; int diffX = slotBounds.getWidth() - e->type.equipscreen_size.x; int diffY = slotBounds.getHeight() - e->type.equipscreen_size.y; switch (alignX) { case VehicleType::AlignmentX::Left: pos.x += 0; break; case VehicleType::AlignmentX::Right: pos.x += diffX * EQUIP_GRID_SLOT_SIZE.x; break; case VehicleType::AlignmentX::Centre: pos.x += (diffX * EQUIP_GRID_SLOT_SIZE.x) / 2; break; } switch (alignY) { case VehicleType::AlignmentY::Top: pos.y += 0; break; case VehicleType::AlignmentY::Bottom: pos.y += diffY * EQUIP_GRID_SLOT_SIZE.y; break; case VehicleType::AlignmentY::Centre: pos.y += (diffY * EQUIP_GRID_SLOT_SIZE.y) / 2; break; } fw().renderer->draw(e->type.equipscreen_sprite, pos); Vec2<int> endPos = pos; endPos.x += e->type.equipscreen_sprite->size.x; endPos.y += e->type.equipscreen_sprite->size.y; this->equippedItems.emplace_back(std::make_pair(Rect<int>{pos, endPos}, e)); } // Only draw inventory that can be used by this type of craft VEquipmentType::User allowedEquipmentUser; switch (this->selected->type.type) { case VehicleType::Type::Flying: allowedEquipmentUser = VEquipmentType::User::Air; break; case VehicleType::Type::Ground: allowedEquipmentUser = VEquipmentType::User::Ground; break; default: LogError( "Trying to draw equipment screen of unsupported vehicle type for vehicle \"%s\"", this->selected->name.c_str()); allowedEquipmentUser = VEquipmentType::User::Air; } // Draw the inventory if the selected is in a building, and that is a base auto bld = this->selected->building.lock(); sp<Base> base; if (bld) { base = bld->base; } if (base) { auto inventoryControl = form->FindControlTyped<Graphic>("INVENTORY"); Vec2<int> inventoryPosition = inventoryControl->Location + form->Location; for (auto &invPair : base->inventory) { // The gap between the bottom of the inventory image and the count label static const int INVENTORY_COUNT_Y_GAP = 4; // The gap between the end of one inventory image and the start of the next static const int INVENTORY_IMAGE_X_GAP = 4; auto equipIt = state->getRules().getVehicleEquipmentTypes().find(invPair.first); if (equipIt == state->getRules().getVehicleEquipmentTypes().end()) { // It's not vehicle equipment, skip continue; } auto &equipmentType = *equipIt->second; if (equipmentType.type != this->selectionType) { // Skip equipment of different types continue; } if (!equipmentType.users.count(allowedEquipmentUser)) { // The selected vehicle is not a valid user of the equipment, don't draw continue; } int count = invPair.second; if (count == 0) { // Not in stock continue; } auto countImage = labelFont->getString(UString::format("%d", count)); auto &equipmentImage = equipmentType.equipscreen_sprite; fw().renderer->draw(equipmentImage, inventoryPosition); Vec2<int> countLabelPosition = inventoryPosition; countLabelPosition.y += INVENTORY_COUNT_Y_GAP + equipmentImage->size.y; // FIXME: Center in X? fw().renderer->draw(countImage, countLabelPosition); Vec2<int> inventoryEndPosition = inventoryPosition; inventoryEndPosition.x += equipmentImage->size.x; inventoryEndPosition.y += equipmentImage->size.y; this->inventoryItems.emplace_back(Rect<int>{inventoryPosition, inventoryEndPosition}, equipmentType); // Progress inventory offset by width of image + gap inventoryPosition.x += INVENTORY_IMAGE_X_GAP + equipmentImage->size.x; } } if (this->drawHighlightBox) { Vec2<int> p00 = highlightBox.p0; Vec2<int> p11 = highlightBox.p1; Vec2<int> p01 = {p00.x, p11.y}; Vec2<int> p10 = {p11.x, p00.y}; fw().renderer->drawLine(p00, p01, highlightBoxColour, 1); fw().renderer->drawLine(p01, p11, highlightBoxColour, 1); fw().renderer->drawLine(p11, p10, highlightBoxColour, 1); fw().renderer->drawLine(p10, p00, highlightBoxColour, 1); } if (this->draggedEquipment) { // Draw equipment we're currently dragging (snapping to the grid if possible) Vec2<int> equipmentPos = fw().gamecore->MouseCursor->getPosition() + this->draggedEquipmentOffset; // If this is within the grid try to snap it Vec2<int> equipmentGridPos = equipmentPos - equipOffset; equipmentGridPos /= EQUIP_GRID_SLOT_SIZE; if (equipmentGridPos.x < 0 || equipmentGridPos.x >= EQUIP_GRID_SLOTS.x || equipmentGridPos.y < 0 || equipmentGridPos.y >= EQUIP_GRID_SLOTS.y) { // This is outside thge grid } else { // Inside the grid, snap equipmentPos = equipmentGridPos * EQUIP_GRID_SLOT_SIZE; equipmentPos += equipOffset; } fw().renderer->draw(this->draggedEquipment->equipscreen_sprite, equipmentPos); } fw().gamecore->MouseCursor->Render(); }
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> void Processor::flush() { size_t n = _samplePool.size(); double *data = _samplePool.samples; bool clipped = false; // Skip empty sample pool if ( n == 0 ) return; // ------------------------------------------------------------------- // Saturation check // ------------------------------------------------------------------- if ( _config.saturationThreshold >= 0 ) { double maxCounts = (_config.saturationThreshold * 0.01) * (2 << 23); for ( size_t i = 0; i < n; ++i ) { if ( fabs(data[i]) > maxCounts ) clipped = true; } } // ------------------------------------------------------------------- // Sensitivity correction // ------------------------------------------------------------------- double scorr = 1.0 / _streamConfig[_usedComponent].gain; for ( size_t i = 0; i < n; ++i ) data[i] *= scorr; // ------------------------------------------------------------------- // Baseline correction and filtering // ------------------------------------------------------------------- double amp0 = getValue(n, data, NULL, _baselineCorrection0, _filter0.get()); // ------------------------------------------------------------------- // Conversion to ACC, VEL, DISP // ------------------------------------------------------------------- SignalUnit unit; if ( !unit.fromString(_streamConfig[_usedComponent].gainUnit.c_str()) ) { SEISCOMP_ERROR("%s: internal error: invalid gain unit '%s'", Private::toStreamID(_waveformID).c_str(), _streamConfig[_usedComponent].gainUnit.c_str()); return; } double vel, acc, disp; std::vector<double> tmp; double *vel_data; switch ( unit ) { case MeterPerSecond: vel = amp0; tmp.assign(data, data+n); vel_data = &tmp[0]; acc = getAcceleration(n, data); break; case MeterPerSecondSquared: acc = amp0; vel = getVelocity(n, data); vel_data = data; break; default: SEISCOMP_ERROR("%s: internal error: unsupported gain unit '%s'", Private::toStreamID(_waveformID).c_str(), _streamConfig[_usedComponent].gainUnit.c_str()); return; } disp = getDisplacement(n, vel_data); // Publish result if ( _func ) _func(this, acc, vel, disp, _currentStartTime, clipped); _samplePool.clear(); }
U32 LLVOTree::processUpdateMessage(LLMessageSystem *mesgsys, void **user_data, U32 block_num, EObjectUpdateType update_type, LLDataPacker *dp) { // Do base class updates... U32 retval = LLViewerObject::processUpdateMessage(mesgsys, user_data, block_num, update_type, dp); if ( (getVelocity().lengthSquared() > 0.f) ||(getAcceleration().lengthSquared() > 0.f) ||(getAngularVelocity().lengthSquared() > 0.f)) { llinfos << "ACK! Moving tree!" << llendl; setVelocity(LLVector3::zero); setAcceleration(LLVector3::zero); setAngularVelocity(LLVector3::zero); } if (update_type == OUT_TERSE_IMPROVED) { // Nothing else needs to be done for the terse message. return retval; } // // Load Instance-Specific data // if (mData) { mSpecies = ((U8 *)mData)[0]; } if (!sSpeciesTable.count(mSpecies)) { if (sSpeciesTable.size()) { SpeciesMap::const_iterator it = sSpeciesTable.begin(); mSpecies = (*it).first; } } // // Load Species-Specific data // static const S32 MAX_TREE_TEXTURE_VIRTURE_SIZE_RESET_INTERVAL = 32 ; //frames. mTreeImagep = LLViewerTextureManager::getFetchedTexture(sSpeciesTable[mSpecies]->mTextureID, TRUE, LLViewerTexture::BOOST_NONE, LLViewerTexture::LOD_TEXTURE); mTreeImagep->setMaxVirtualSizeResetInterval(MAX_TREE_TEXTURE_VIRTURE_SIZE_RESET_INTERVAL); //allow to wait for at most 16 frames to reset virtual size. mBranchLength = sSpeciesTable[mSpecies]->mBranchLength; mTrunkLength = sSpeciesTable[mSpecies]->mTrunkLength; mLeafScale = sSpeciesTable[mSpecies]->mLeafScale; mDroop = sSpeciesTable[mSpecies]->mDroop; mTwist = sSpeciesTable[mSpecies]->mTwist; mBranches = sSpeciesTable[mSpecies]->mBranches; mDepth = sSpeciesTable[mSpecies]->mDepth; mScaleStep = sSpeciesTable[mSpecies]->mScaleStep; mTrunkDepth = sSpeciesTable[mSpecies]->mTrunkDepth; mBillboardScale = sSpeciesTable[mSpecies]->mBillboardScale; mBillboardRatio = sSpeciesTable[mSpecies]->mBillboardRatio; mTrunkAspect = sSpeciesTable[mSpecies]->mTrunkAspect; mBranchAspect = sSpeciesTable[mSpecies]->mBranchAspect; // position change not caused by us, etc. make sure to rebuild. gPipeline.markRebuild(mDrawable, LLDrawable::REBUILD_ALL); return retval; }
double GUIVehicle::getColorValue(int activeScheme) const { switch (activeScheme) { case 8: return getSpeed(); case 9: // color by action step if (isActionStep(SIMSTEP)) { // Upcoming simstep is actionstep (t was already increased before drawing) return 1.; } else if (isActive()) { // Completed simstep was actionstep return 2.; } else { // not active return 0.; } case 10: return getWaitingSeconds(); case 11: return getAccumulatedWaitingSeconds(); case 12: return getLastLaneChangeOffset(); case 13: return getLane()->getVehicleMaxSpeed(this); case 14: return getCO2Emissions(); case 15: return getCOEmissions(); case 16: return getPMxEmissions(); case 17: return getNOxEmissions(); case 18: return getHCEmissions(); case 19: return getFuelConsumption(); case 20: return getHarmonoise_NoiseEmissions(); case 21: if (getNumberReroutes() == 0) { return -1; } return getNumberReroutes(); case 22: return gSelected.isSelected(GLO_VEHICLE, getGlID()); case 23: return getLaneChangeModel().isOpposite() ? -100 : getBestLaneOffset(); case 24: return getAcceleration(); case 25: return getTimeGapOnLane(); case 26: return STEPS2TIME(getDepartDelay()); case 27: return getElectricityConsumption(); case 28: return getTimeLossSeconds(); case 29: return getLaneChangeModel().getSpeedLat(); } return 0; }
/** \brief Loop for state polling in modes script and polling. Also sends command in script mode. */ void timer_cb(const ros::TimerEvent& ev) { // ==== Get state values by built-in commands ==== gripper_response info; float acc = 0.0; info.speed = 0.0; if (g_mode_polling) { const char * state = systemState(); if (!state) return; info.state_text = std::string(state); info.position = getOpening(); acc = getAcceleration(); info.f_motor = getForce();//getGraspingForce(); } else if (g_mode_script) { // ==== Call custom measure-and-move command ==== int res = 0; if (!std::isnan(g_goal_position)) { ROS_INFO("Position command: pos=%5.1f, speed=%5.1f", g_goal_position, g_speed); res = script_measure_move(1, g_goal_position, g_speed, info); } else if (!std::isnan(g_goal_speed)) { ROS_INFO("Velocity command: speed=%5.1f", g_goal_speed); res = script_measure_move(2, 0, g_goal_speed, info); } else res = script_measure_move(0, 0, 0, info); if (!std::isnan(g_goal_position)) g_goal_position = NAN; if (!std::isnan(g_goal_speed)) g_goal_speed = NAN; if (!res) { ROS_ERROR("Measure-and-move command failed"); return; } // ==== Moving msg ==== if (g_ismoving != info.ismoving) { std_msgs::Bool moving_msg; moving_msg.data = info.ismoving; g_pub_moving.publish(moving_msg); g_ismoving = info.ismoving; } } else return; // ==== Status msg ==== wsg_50_common::Status status_msg; status_msg.status = info.state_text; status_msg.width = info.position; status_msg.speed = info.speed; status_msg.acc = acc; status_msg.force = info.f_motor; status_msg.force_finger0 = info.f_finger0; status_msg.force_finger1 = info.f_finger1; g_pub_state.publish(status_msg); // ==== Joint state msg ==== // \todo Use name of node for joint names sensor_msgs::JointState joint_states; joint_states.header.stamp = ros::Time::now();; joint_states.header.frame_id = "summit_xl_wsg50_base_link"; joint_states.name.push_back("summit_xl_wsg50_finger_left_joint"); joint_states.name.push_back("summit_xl_wsg50_finger_right_joint"); joint_states.position.resize(2); joint_states.position[0] = -info.position/2000.0; joint_states.position[1] = info.position/2000.0; joint_states.velocity.resize(2); joint_states.velocity[0] = info.speed/1000.0; joint_states.velocity[1] = info.speed/1000.0; joint_states.effort.resize(2); joint_states.effort[0] = info.f_motor; joint_states.effort[1] = info.f_motor; g_pub_joint.publish(joint_states); // printf("Timer, last duration: %6.1f\n", ev.profile.last_duration.toSec() * 1000.0); }
void ADynamicCarController::Tick(float DeltaSeconds) { Super::Tick(DeltaSeconds); if (play) { float deltaSec = GWorld->GetWorld()->GetDeltaSeconds(); updateTarget(); if (first) { first = false; FRotator rotation = agent->GetActorRotation(); rotation.Yaw = getRotation(agent->GetActorLocation(), target); agent->SetActorRotation(rotation); if (followPath) { waypoints = DubinsPath::getPath(waypoints, to2D(agent->GetActorLocation()), rotation.Yaw, maxAngle, L, graph, errorTolerance); writeWaypointsToFile("Waypoints2.txt"); if (waypoints.Num() > 0) { target = waypoints[0]; } } } if (waypointReached()) { bool t35 = followPath && waypointsIndex >= waypoints.Num(); bool t4 = avoidAgents && !followPath; if (t35 || t4) { play = false; GEngine->AddOnScreenDebugMessage(-1, 50.f, FColor::Green, FString::Printf(TEXT("Time: %f\r\n"), totalTime)); } return; } float a = getAcceleration(deltaSec); v += a; v = UKismetMathLibrary::FClamp(v, -vMax, vMax); float rotation = rotate(deltaSec); FVector vPref = FVector(v * UKismetMathLibrary::DegCos(rotation) * deltaSec, v * UKismetMathLibrary::DegSin(rotation) * deltaSec, 0); vPref = vPref.GetClampedToMaxSize2D(UKismetMathLibrary::FMin(v * deltaSec, FVector2D::Distance(to2D(agent->GetActorLocation()), target))); FVector oldVel = velocity; if (avoidAgents) { adjustVelocity(to2D(vPref), deltaSec); //velocity = vPref; } else { velocity = vPref; } FVector2D q = to2D(velocity - oldVel); float dv = FMath::Sqrt(FVector2D::DotProduct(q, q)); if (dv > aMax * deltaSec) { float f = aMax * deltaSec / dv; velocity = (1 - f) * oldVel + f * velocity; } float rot = agent->GetActorRotation().Yaw; if (velocity.Size2D() != 0) { if (angleDiff(rot, velocity.Rotation().Yaw) > 90) { velocity = -velocity; setRotation(); velocity = -velocity; } else { setRotation(); } } /* if (UKismetMathLibrary::Abs(angleDiff(rot, agent->GetActorRotation().Yaw)) / deltaSec > maxAngle * (v / L) + 5) { GEngine->AddOnScreenDebugMessage(-1, 50.f, FColor::Red, FString::Printf(TEXT("rot: %f yaw: %f -> %f (%f) %d %s -> %s"), rot, agent->GetActorRotation().Yaw, angleDiff(rot, agent->GetActorRotation().Yaw), UKismetMathLibrary::Abs(angleDiff(rot, agent->GetActorRotation().Yaw)) / deltaSec, vPref.Equals(velocity), *to2D(vPref).ToString(), *to2D(velocity).ToString())); DrawDebugLine(GWorld->GetWorld(), FVector(to2D(agent->GetActorLocation()), 20), FVector(to2D(agent->GetActorLocation()), 30), FColor::Red, false, 0.5, 0, 1); GEngine->DeferredCommands.Add(TEXT("pause")); }*/ agent->SetActorLocation(agent->GetActorLocation() + velocity); /* DrawDebugLine(GWorld->GetWorld(), to3D(target), to3D(target) + collisionSize, collisionColor, false, 0.1, 0, 1); float a = getAcceleration(deltaSec); v += a; v = UKismetMathLibrary::FClamp(v, -vMax, vMax); float rotation = rotate(deltaSec); acceleration.X = a * UKismetMathLibrary::DegCos(rotation); acceleration.Y = a * UKismetMathLibrary::DegSin(rotation); drawLine(2 * acceleration / deltaSec, accelerationColor); velocity = FVector(v * UKismetMathLibrary::DegCos(rotation) * deltaSec, v * UKismetMathLibrary::DegSin(rotation) * deltaSec, 0); float rot = agent->GetActorRotation().Yaw; setRotation(); GEngine->AddOnScreenDebugMessage(-1, 50.f, FColor::Red, FString::Printf(TEXT("%f"), UKismetMathLibrary::Abs(rot - agent->GetActorRotation().Yaw) / deltaSec)); agent->SetActorLocation(agent->GetActorLocation() + velocity); DrawDebugLine(GWorld->GetWorld(), agent->GetActorLocation(), agent->GetActorLocation() + collisionSize, FColor::Green, false, 0.1, 0, 1); */ } }
int main( int argc, char **argv ) { ros::init(argc, argv, "wsg_50"); ros::NodeHandle nh; std::string ip; int port; ROS_INFO("WSG 50 - ROS NODE"); nh.param("wsg_50_tcp/ip", ip, std::string("192.168.1.20")); nh.param("wsg_50_tcp/port", port, 1000); // Connect to device using TCP if( cmd_connect_tcp( ip.c_str(), port ) == 0 ) { ROS_INFO("TCP connection stablished"); // Services ros::ServiceServer moveSS = nh.advertiseService("wsg_50/move", moveSrv); ros::ServiceServer graspSS = nh.advertiseService("wsg_50/grasp", graspSrv); ros::ServiceServer releaseSS = nh.advertiseService("wsg_50/release", releaseSrv); ros::ServiceServer homingSS = nh.advertiseService("wsg_50/homing", homingSrv); ros::ServiceServer stopSS = nh.advertiseService("wsg_50/stop", stopSrv); ros::ServiceServer ackSS = nh.advertiseService("wsg_50/ack", ackSrv); ros::ServiceServer incrementSS = nh.advertiseService("wsg_50/move_incrementally", incrementSrv); ros::ServiceServer setAccSS = nh.advertiseService("wsg_50/set_acceleration", setAccSrv); ros::ServiceServer setForceSS = nh.advertiseService("wsg_50/set_force", setForceSrv); // Publisher ros::Publisher state_pub = nh.advertise<wsg_50_common::Status>("wsg_50/status", 1000); ros::Publisher joint_states_pub = nh.advertise<sensor_msgs::JointState>("/joint_states", 10); ROS_INFO("Ready to use."); homing(); ros::Rate loop_rate(1); // loop at 1Hz while( ros::ok() ){ //Loop waiting for orders and updating the state //Create the msg to send wsg_50_common::Status status_msg; //Get state values const char * aux; aux = systemState(); int op = getOpening(); int acc = getAcceleration(); int force = getGraspingForceLimit(); std::stringstream ss; ss << aux; status_msg.status = ss.str(); status_msg.width = op; status_msg.acc = acc; status_msg.force = force; state_pub.publish(status_msg); sensor_msgs::JointState joint_states; joint_states.header.stamp = ros::Time::now();; joint_states.header.frame_id = "wsg_50_gripper_base_link"; joint_states.name.push_back("wsg_50_gripper_base_joint_gripper_left"); joint_states.name.push_back("wsg_50_gripper_base_joint_gripper_right"); joint_states.position.resize(2); joint_states.position[0] = -op/2000.0; joint_states.position[1] = op/2000.0; //joint_states.velocity.resize(2); //joint_states.velocity[0]; //joint_states.velocity[1]; joint_states.effort.resize(2); joint_states.effort[0] = force; joint_states.effort[1] = force; joint_states_pub.publish(joint_states); loop_rate.sleep(); ros::spinOnce(); } }else{ ROS_ERROR("Unable to connect via TCP, please check the port and address used."); } // Disconnect - won't be executed atm. as the endless loop in test() // will never return. cmd_disconnect(); return 0; }