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