/*
================
idSplinePosition::start
================
*/
void idSplinePosition::start( long t ) {
idCameraPosition::start( t );
target.initPosition(t, time);
lastTime = startTime;
distSoFar = 0.0f;
calcVelocity(target.totalDistance());
}
/*
================
idInterpolatedPosition::start
================
*/
void idInterpolatedPosition::start( long t ) {
idCameraPosition::start(t);
lastTime = startTime;
distSoFar = 0.0f;
idVec3 temp = startPos;
temp -= endPos;
calcVelocity(temp.Length());
}
// What we want to do here is to look at all the points that are *not* included in the indices list,
// (i.e. these are the outliers, the points not on the floor) and determine which are the closest to the camera
void cloudIndicesModelCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
const PointIndicesConstPtr& indices, const pcl::ModelCoefficientsConstPtr& model)
{
boost::mutex::scoped_lock lock (callback_mutex);
NODELET_DEBUG_STREAM("Got cloud with timestamp " << cloud_msg->header.stamp << " + indices with timestamp "
<< indices->header.stamp << " + model with timestamp " << model->header.stamp);
double dt;
if (first)
{
first = false;
dt = 0;
}
else
{
dt = (cloud_msg->header.stamp - prev_cloud_time).toSec();
prev_cloud_time = cloud_msg->header.stamp;
}
if (model->values.size() > 0)
{
// Determine altitude:
kinect_z = reject_outliers(-fabs(model->values[3])) - imu_to_kinect_offset;
calcVelocity(kinect_z, dt, kinect_vz);
// Detect obstacles:
pcl::PointXYZ obstacle_location;
bool obstacle_found = detectObstacle(cloud_msg, indices, model, obstacle_location);
if (obstacle_found)
{
publishObstacleMarker(obstacle_location);
NODELET_DEBUG("Detected obstacle at: [%f, %f, %f]", obstacle_location.x, obstacle_location.y,
obstacle_location.z);
}
publishObstacle(obstacle_found, obstacle_location);
}
else
{
NODELET_WARN("No planar model found -- cannot determine altitude or obstacles");
}
updateMask();
if (not use_backup_estimator_alt)
{
publishOdom();
}
if (debug_throttle_rate > 0)
{
ros::Duration(1 / debug_throttle_rate).sleep();
}
}
void Foam::KinematicParcel<ParcelType>::calc
(
TrackData& td,
const scalar dt,
const label cellI
)
{
// Define local properties at beginning of time step
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
const scalar np0 = nParticle_;
const scalar d0 = d_;
const vector U0 = U_;
const scalar rho0 = rho_;
const scalar mass0 = mass();
// Reynolds number
const scalar Re = this->Re(U0, d0, rhoc_, muc_);
// Sources
//~~~~~~~~
// Explicit momentum source for particle
vector Su = vector::zero;
// Momentum transfer from the particle to the carrier phase
vector dUTrans = vector::zero;
// Motion
// ~~~~~~
// Calculate new particle velocity
vector U1 =
calcVelocity(td, dt, cellI, Re, muc_, d0, U0, rho0, mass0, Su, dUTrans);
// Accumulate carrier phase source terms
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
if (td.cloud().coupled())
{
// Update momentum transfer
td.cloud().UTrans()[cellI] += np0*dUTrans;
}
// Set new particle properties
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~
U_ = U1;
}
void NPC::update(float frameTime)
{
calcVelocity(frameTime);
spriteData.x += velocity.x * frameTime;
spriteData.y += velocity.y * frameTime;
// update Image to animate
if(velocity.x == 0.0 && velocity.y == 0.0) {
// don't animate if not moving
setLoop(false);
} else {
// animate
setLoop(true);
Entity::update(frameTime);
}
}
void Foam::KinematicParcel<ParcelType>::calc
(
TrackData& td,
const scalar dt,
const label cellI
)
{
// Define local properties at beginning of time step
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
const scalar np0 = nParticle_;
const scalar mass0 = mass();
// Reynolds number
const scalar Re = this->Re(U_, d_, rhoc_, muc_);
// Sources
//~~~~~~~~
// Explicit momentum source for particle
vector Su = vector::zero;
// Linearised momentum source coefficient
scalar Spu = 0.0;
// Momentum transfer from the particle to the carrier phase
vector dUTrans = vector::zero;
// Motion
// ~~~~~~
// Calculate new particle velocity
this->U_ = calcVelocity(td, dt, cellI, Re, muc_, mass0, Su, dUTrans, Spu);
// Accumulate carrier phase source terms
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
if (td.cloud().solution().coupled())
{
// Update momentum transfer
td.cloud().UTrans()[cellI] += np0*dUTrans;
// Update momentum transfer coefficient
td.cloud().UCoeff()[cellI] += np0*Spu;
}
}
void
ZouHeDynamics::defineDensity(DataType& data, DensityType density) const noexcept
{
const auto velocity = calcVelocity(data, density);
init(data, velocity, density);
}
