Go to the documentation of this file.00001
00036 #include <boost/bind.hpp>
00037 #include <string>
00038
00039 #include "ros/ros.h"
00040 #include "enu/FromFix.h"
00041 #include "enu/ToFix.h"
00042 #include "enu/ToENU.h"
00043 #include "enu/enu.h"
00044
00045 bool from_fix_service(const enu::FromFix::Request& req, enu::FromFix::Response& resp) {
00046 enu::fix_to_point(req.llh, req.datum, &(resp.point));
00047 return true;
00048 }
00049
00050 bool to_fix_service(const enu::ToFix::Request& req, enu::ToFix::Response& resp) {
00051 enu::point_to_fix(req.point, req.datum, &(resp.llh));
00052 return true;
00053 }
00054
00055 int main(int argc, char **argv) {
00056 ros::init(argc, argv, "enu_services");
00057 ros::NodeHandle n;
00058
00059 ros::ServiceServer from_fix_srv =
00060 n.advertiseService<enu::FromFix::Request, enu::FromFix::Response> ("from_fix", from_fix_service);
00061 ros::ServiceServer to_fix_srv =
00062 n.advertiseService<enu::ToFix::Request, enu::ToFix::Response> ("to_fix", to_fix_service);
00063
00064 ros::spin();
00065 return 0;
00066 }
00067