geometry_msgs::PoseArray PFilterTest::getParticlePoseArray(int idx)
{
std::vector<cspace> particles;
pFilter_.getAllParticles(particles, idx);
// tf::Transform trans = pHandler.getTransformToPartFrame();
tf::Transform trans = trans_;
if (idx == 5) trans = trans1_;
#ifdef POINT_CLOUD
boost::mutex::scoped_lock updateLock(updateModelMutex);
basic_cloud_ptr1->points.clear();
basic_cloud_ptr2->points.clear();
int numParticles = pFilter_.getNumParticles();
for (int j = 0; j < numParticles; 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
// cspace particles_est_stat;
// cspace particles_est;
// pFilter_.estimateGaussian(particles_est, particles_est_stat);
geometry_msgs::PoseArray poseArray;
for(int i=0; i<50; i++){
tf::Pose pose = poseAt(particles[i]);
geometry_msgs::Pose pose_msg;
tf::poseTFToMsg(trans*pose, pose_msg);
poseArray.poses.push_back(pose_msg);
// ROS_INFO("Pose %d: %f, %f, %f", i, poseArray.poses[i].position.x,
// poseArray.poses[i].position.y,
// poseArray.poses[i].position.z);
}
return poseArray;
}
geometry_msgs::PoseArray PFilterTest::getParticlePoseArray()
{
particleFilter::cspace particles[NUM_PARTICLES];
pFilter_.getAllParticles(particles);
tf::Transform trans = plt.getTrans();
#ifdef POINT_CLOUD
boost::mutex::scoped_lock updateLock(updateModelMutex);
basic_cloud_ptr1->points.clear();
basic_cloud_ptr2->points.clear();
for (int j = 0; j < pFilter_.numParticles; 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
particleFilter::cspace particles_est_stat;
particleFilter::cspace particles_est;
pFilter_.estimatedDistribution(particles_est, particles_est_stat);
geometry_msgs::PoseArray poseArray;
for(int i=0; i<50; i++){
tf::Pose pose = poseAt(particles[i]);
geometry_msgs::Pose pose_msg;
tf::poseTFToMsg(trans*pose, pose_msg);
poseArray.poses.push_back(pose_msg);
// ROS_INFO("Pose %d: %f, %f, %f", i, poseArray.poses[i].position.x,
// poseArray.poses[i].position.y,
// poseArray.poses[i].position.z);
}
return poseArray;
}
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());
}
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;
}
