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