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