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;
}
