void philoStart(int shareID,int pid){
//starts the looping of eating and thinking
srand(getpid());
int eatDur;
int timeEaten = 0;
struct sembuf takeChop[2] = {{pid,-1,0},{((pid+1)%5),-1,0}};
struct sembuf giveChop[2] = {{pid,1,0},{((pid+1)%5),1,0}};
while(timeEaten < 100){
//wait on my two semaphores
if(semop(shareID,takeChop,2)==-1){
errorHandler();
};
//got them, time to eat
eatDur = randomGaussian(9,3);
if (eatDur < 0)eatDur = 0;
printf("I am Philospher %d and I am eating for %d seconds, so far ive eaten for %d seconds\n",pid,eatDur,timeEaten);
timeEaten += eatDur;
sleep(eatDur);
if(semop(shareID,giveChop,2)==-1){
errorHandler();
};
//done eating time to sleep
eatDur = randomGaussian(11,7);
if (eatDur < 0)eatDur = 0;
printf("I am Philospher %d and I am thinking for %d seconds\n",pid,eatDur);
sleep(eatDur);
}
printf("I am Philospher %d and I an done eating\n",pid);
exit (0);
}
pcl::tracking::ParticleCuboid PlaneSupportedCuboidEstimator::sample(const pcl::tracking::ParticleCuboid& p)
{
pcl::tracking::ParticleCuboid sampled_particle;
// Motion model is tricky.
// It's not a tracking problem, so we need to limit noise of motion model
// The new particle should satisfy:
// 1. within a polygon
// 2. within local z range
// 3. within local pitch/roll range
// 4. within world yaw range
// 5. within dx/dy/dz range
//sampled_particle = p;
sampled_particle.x = randomGaussian(p.x, step_x_variance_, random_generator_);
sampled_particle.y = randomGaussian(p.y, step_y_variance_, random_generator_);
sampled_particle.z = randomGaussian(p.z, step_z_variance_, random_generator_);
sampled_particle.roll = randomGaussian(p.roll, step_roll_variance_, random_generator_);
sampled_particle.pitch = randomGaussian(p.pitch, step_pitch_variance_, random_generator_);
sampled_particle.yaw = randomGaussian(p.yaw, step_yaw_variance_, random_generator_);
sampled_particle.dx = std::max(randomGaussian(p.dx, step_dx_variance_, random_generator_),
min_dx_);
sampled_particle.dy = std::max(randomGaussian(p.dy, step_dy_variance_, random_generator_),
min_dy_);
sampled_particle.dz = std::max(randomGaussian(p.dz, step_dz_variance_, random_generator_),
min_dz_);
sampled_particle.plane_index = p.plane_index;
sampled_particle.weight = p.weight;
return sampled_particle;
}
void philoCommand(int num) {
int leftStick, rightStick;
int thinkTime = 0;
int totalTime = 0;
int eatTime = 0;
int totalTimeToEat = TOTAL_EAT_TIME;
leftStick = num;
totalTime = 0;
srand((int)time(NULL) + num);
if(num == PHILOSOPHERS - 1) {
rightStick = 0;
} else {
rightStick = num + 1;
}
while(totalTimeToEat > 0) {
if((thinkTime = randomGaussian(T_MEAN, T_STDEV)) < 0) {
thinkTime = 0;
}
printf("Philo %d will think for %d seconds (%d seconds total)\n",
num, thinkTime, totalTime);
totalTime += thinkTime;
sleep(thinkTime);
if((eatTime = randomGaussian(T_MEAN, T_STDEV)) < 0) {
eatTime = 0;
}
if((pickUp(leftStick, rightStick) == -1) && (errno == EINTR)) {
fprintf(stderr, "Error with semop locking: %s\n", strerror(errno));
exit(1);
}
printf("Philo %d will eat for %d seconds (%d seconds total)\n",
num, eatTime, TOTAL_EAT_TIME - totalTimeToEat);
totalTimeToEat -= eatTime;
sleep(eatTime);
if((putDown(leftStick, rightStick) == -1) && (errno == EINTR)) {
fprintf(stderr, "Error with semop unlocking: %s\n", strerror(errno));
exit(1);
}
}
printf("Philosopher %d has finished eating for %d and thinking for %d ",
num, TOTAL_EAT_TIME - totalTimeToEat, totalTime);
exit(0);
}
pcl::PointCloud<pcl::tracking::ParticleCuboid>::Ptr PlaneSupportedCuboidEstimator::initParticles()
{
pcl::PointCloud<pcl::tracking::ParticleCuboid>::Ptr particles (new pcl::PointCloud<pcl::tracking::ParticleCuboid>);
particles->points.resize(particle_num_);
std::vector<Polygon::Ptr> polygons(latest_polygon_msg_->polygons.size());
for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) {
Polygon::Ptr polygon = Polygon::fromROSMsgPtr(latest_polygon_msg_->polygons[i].polygon);
polygons[i] = polygon;
}
for (size_t i = 0; i < particle_num_; i++) {
while (true) {
pcl::tracking::ParticleCuboid p_local;
size_t plane_i = chooseUniformRandomPlaneIndex(polygons);
Polygon::Ptr polygon = polygons[plane_i];
Eigen::Vector3f v = polygon->randomSampleLocalPoint(random_generator_);
v[2] = randomUniform(init_local_position_z_min_, init_local_position_z_max_,
random_generator_);
p_local.getVector3fMap() = v;
p_local.roll = randomGaussian(0, init_local_orientation_roll_variance_, random_generator_);
p_local.pitch = randomGaussian(0, init_local_orientation_pitch_variance_, random_generator_);
p_local.yaw = randomGaussian(init_local_orientation_yaw_mean_,
init_local_orientation_yaw_variance_,
random_generator_);
p_local.dx = randomGaussian(init_dx_mean_, init_dx_variance_, random_generator_);
p_local.dy = randomGaussian(init_dy_mean_, init_dy_variance_, random_generator_);
p_local.dz = randomGaussian(init_dz_mean_, init_dz_variance_, random_generator_);
pcl::tracking::ParticleCuboid p_global = p_local * polygon->coordinates();
if (use_init_world_position_z_model_) {
if (p_global.z < init_world_position_z_min_ ||
p_global.z > init_world_position_z_max_) {
continue;
}
}
p_global.plane_index = plane_i;
if (disable_init_pitch_) {
p_global.pitch = 0;
}
if (disable_init_roll_) {
p_global.roll = 0;
}
particles->points[i] = p_global;
break;
}
}
return particles;
}
