TEST (BagTest, PoseCheck)
{
ros::NodeHandle nh;
ros::NodeHandle nhLocal("~");
double finalX = 0;
double finalY = 0;
double finalZ = 0;
double tolerance = 0;
bool outputFinalPosition = false;
std::string finalPositionFile;
nhLocal.getParam("final_x", finalX);
nhLocal.getParam("final_y", finalY);
nhLocal.getParam("final_z", finalZ);
nhLocal.getParam("tolerance", tolerance);
nhLocal.param("output_final_position", outputFinalPosition, false);
nhLocal.param("output_location", finalPositionFile, std::string("test.txt"));
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
while(ros::ok())
{
ros::spinOnce();
ros::Duration(0.0333333).sleep();
}
if(outputFinalPosition)
{
try
{
std::ofstream posOut;
posOut.open(finalPositionFile.c_str(), std::ofstream::app);
posOut << filtered_.pose.pose.position.x << " " << filtered_.pose.pose.position.y << " " << filtered_.pose.pose.position.z << std::endl;
posOut.close();
}
catch(...)
{
ROS_ERROR_STREAM("Unable to open output file.\n");
}
}
double xDiff = filtered_.pose.pose.position.x - finalX;
double yDiff = filtered_.pose.pose.position.y - finalY;
double zDiff = filtered_.pose.pose.position.z - finalZ;
EXPECT_LT(::sqrt(xDiff*xDiff + yDiff*yDiff + zDiff*zDiff), tolerance);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ukf_navigation_node");
ros::NodeHandle nhLocal("~");
std::vector<double> args(3, 0);
nhLocal.param("alpha", args[0], 0.001);
nhLocal.param("kappa", args[1], 0.0);
nhLocal.param("beta", args[2], 2.0);
RobotLocalization::RosUkf ukf(args);
ukf.run();
return 0;
}
