bool PFilterTest::addObs(particle_filter::AddObservation::Request &req,
particle_filter::AddObservation::Response &resp)
{
geometry_msgs::Point obs = req.p;
geometry_msgs::Point dir = req.dir;
int datum = req.datum;
ROS_INFO("Current update datum: %d", datum);
ROS_INFO("Adding Observation...") ;
ROS_INFO("point: %f, %f, %f", obs.x, obs.y, obs.z);
ROS_INFO("dir: %f, %f, %f", dir.x, dir.y, dir.z);
double obs2[2][3] = {{obs.x, obs.y, obs.z}, {dir.x, dir.y, dir.z}};
pFilter_.addObservation(obs2, mesh, dist_transform, 0, datum);
ROS_INFO("...Done adding observation");
pub_particles.publish(getParticlePoseArray(0));
pub_particles1.publish(getParticlePoseArray(5));
pub_particles2.publish(getParticlePoseArray(6));
return true;
}
PFilterTest::PFilterTest(int n_particles, particleFilter::cspace b_init[2]) :
pFilter_(n_particles, b_init, 0.001, 0.0035, 0.0001, 0.001),
num_voxels{200, 200, 200}//,
//dist_transform(num_voxels)
// particleFilter (int n_particles, cspace b_init[2],
// double Xstd_ob=0.0001 (measurement error),
// double Xstd_tran=0.0025, (gausian kernel sampling std
// double Xstd_scatter=0.0001, (scatter particles a little before computing mean of distance transform
// double R=0.01) (probe radius);
{
// sub_init = n.subscribe("/particle_filter_init", 1, &PFilterTest::initDistribution, this);
srv_add_obs = n.advertiseService("/particle_filter_add", &PFilterTest::addObs, this);
pub_particles = n.advertise<geometry_msgs::PoseArray>("/particles_from_filter", 5);
ROS_INFO("Loading Boeing Particle Filter");
getMesh("boeing_part.stl");
//int num_voxels[3] = { 200,200,200 };
//dist_transform(num_voxels);
ROS_INFO("start create dist_transform");
dist_transform = new distanceTransform(num_voxels);
#ifdef POINT_CLOUD
particleFilter::cspace particles[NUM_PARTICLES];
pFilter_.getAllParticles(particles);
boost::mutex::scoped_lock updateLock(updateModelMutex);
basic_cloud_ptr1->points.clear();
basic_cloud_ptr2->points.clear();
for (int j = 0; j < NUM_PARTICLES; j++ ) {
pcl::PointXYZ basic_point;
basic_point.x = particles[j][0] * 2;
basic_point.y = particles[j][1] * 2;
basic_point.z = particles[j][2] * 2;
basic_cloud_ptr1->points.push_back(basic_point);
basic_point.x = particles[j][3] * 2;
basic_point.y = particles[j][4] * 2;
basic_point.z = particles[j][5] * 2;
basic_cloud_ptr2->points.push_back(basic_point);
}
basic_cloud_ptr1->width = (int) basic_cloud_ptr1->points.size ();
basic_cloud_ptr1->height = 1;
basic_cloud_ptr2->width = (int) basic_cloud_ptr2->points.size ();
basic_cloud_ptr2->height = 1;
update = true;
updateLock.unlock();
#endif
ros::Duration(1.0).sleep();
pub_particles.publish(getParticlePoseArray());
}
bool PFilterTest::addObs(particle_filter::AddObservation::Request &req,
particle_filter::AddObservation::Response &resp)
{
geometry_msgs::Point obs = req.p;
geometry_msgs::Point dir = req.dir;
ROS_INFO("Adding Observation...") ;
ROS_INFO("point: %f, %f, %f", obs.x, obs.y, obs.z);
ROS_INFO("dir: %f, %f, %f", dir.x, dir.y, dir.z);
double obs2[2][3] = {{obs.x, obs.y, obs.z}, {dir.x, dir.y, dir.z}};
pFilter_.addObservation(obs2, mesh, dist_transform, 0);
ROS_INFO("...Done adding observation");
pub_particles.publish(getParticlePoseArray());
}
PFilterTest::PFilterTest(int n_particles, cspace b_init[2]) :
pFilter_(n_particles, b_init, 0.0005, 0.000),
num_voxels{200, 200, 200}//,
// pFilter_(n_particles, b_init, 0.001, 0.0025, 0.0001, 0.00),
// num_voxels{300, 300, 300}//,
//dist_transform(num_voxels)
// particleFilter (int n_particles, cspace b_init[2],
// double Xstd_ob=0.0001 (measurement error),
// double Xstd_tran=0.0025, (gausian kernel sampling std
// double Xstd_scatter=0.0001, (scatter particles a little before computing mean of distance transform
// double R=0.01) (probe radius);
{
if(!n.getParam("localization_object", cadName)){
ROS_INFO("Failed to get param: localization_object");
}
// sub_init = n.subscribe("/particle_filter_init", 1, &PFilterTest::initDistribution, this);
sub_request_particles = n.subscribe("/request_particles", 1, &PFilterTest::sendParticles, this);
srv_add_obs = n.advertiseService("/particle_filter_add", &PFilterTest::addObs, this);
pub_particles = n.advertise<geometry_msgs::PoseArray>("/particles_from_filter", 5);
pub_particles1 = n.advertise<geometry_msgs::PoseArray>("/right_datum/particles_from_filter", 5);
pub_particles2 = n.advertise<geometry_msgs::PoseArray>("/left_datum/particles_from_filter", 5);
ROS_INFO("Loading Boeing Particle Filter");
// getMesh("boeing_part.stl");
getMesh("wood_boeing.stl");
// getMesh("part.stl");
//int num_voxels[3] = { 200,200,200 };
//dist_transform(num_voxels);
ROS_INFO("start create dist_transform");
dist_transform = new distanceTransform(num_voxels);
tf::TransformListener tf_listener_;
tf_listener_.waitForTransform("/my_frame", "wood_boeing", ros::Time(0), ros::Duration(10.0));
tf_listener_.lookupTransform("wood_boeing", "/my_frame", ros::Time(0), trans_);
tf_listener_.waitForTransform("/my_frame", "right_datum", ros::Time(0), ros::Duration(10.0));
tf_listener_.lookupTransform("right_datum", "/my_frame", ros::Time(0), trans1_);
ROS_INFO("finish create dist_transform");
#ifdef POINT_CLOUD
std::vector<cspace> particles;
pFilter_.getAllParticles(particles, 0);
boost::mutex::scoped_lock updateLock(updateModelMutex);
basic_cloud_ptr1->points.clear();
basic_cloud_ptr2->points.clear();
for (int j = 0; j < NUM_PARTICLES; j++ ) {
pcl::PointXYZ basic_point;
basic_point.x = particles[j][0] * 2;
basic_point.y = particles[j][1] * 2;
basic_point.z = particles[j][2] * 2;
basic_cloud_ptr1->points.push_back(basic_point);
basic_point.x = particles[j][3] * 2;
basic_point.y = particles[j][4] * 2;
basic_point.z = particles[j][5] * 2;
basic_cloud_ptr2->points.push_back(basic_point);
}
basic_cloud_ptr1->width = (int) basic_cloud_ptr1->points.size ();
basic_cloud_ptr1->height = 1;
basic_cloud_ptr2->width = (int) basic_cloud_ptr2->points.size ();
basic_cloud_ptr2->height = 1;
update = true;
updateLock.unlock();
#endif
ros::Duration(1.0).sleep();
pub_particles.publish(getParticlePoseArray(0));
pub_particles1.publish(getParticlePoseArray(5));
pub_particles2.publish(getParticlePoseArray(6));
}
void PFilterTest::sendParticles(std_msgs::Empty emptyMsg)
{
pub_particles.publish(getParticlePoseArray(0));
pub_particles1.publish(getParticlePoseArray(5));
pub_particles2.publish(getParticlePoseArray(6));
}
