Go to the documentation of this file.00001 #include <ros/ros.h>
00002
00003 #include <place_matcher_msgs/PolygonDissimilarity.h>
00004
00005 bool dissimilarity(place_matcher_msgs::PolygonDissimilarity::Request &req,
00006 place_matcher_msgs::PolygonDissimilarity::Response &res)
00007 {
00008 float diff = 0;
00009 ros::Time start = ros::Time::now();
00010 ROS_DEBUG("request: polygon1.size = %zu, polygon2.size = %zu", req.polygon1.points.size(), req.polygon2.points.size());
00011 for (size_t i = 0; i < req.polygon1.points.size(); i++)
00012 {
00013 for (size_t j = 0; j < req.polygon2.points.size(); j++)
00014 {
00015 diff += req.polygon1.points[i].x - req.polygon2.points[j].x;
00016 }
00017 }
00018 res.raw_dissimilarity = diff / (req.polygon1.points.size() + req.polygon2.points.size());
00019 res.processing_time = ros::Time::now() - start;
00020 ROS_DEBUG("sending back response in %f s", res.processing_time.toSec());
00021
00022 return true;
00023 }
00024
00025 int main(int argc, char **argv)
00026 {
00027 ros::init(argc, argv, "simple_polygon_dissimilarity_server");
00028 ros::NodeHandle nh("~");
00029
00030 ros::ServiceServer service = nh.advertiseService("compute_dissimilarity", dissimilarity);
00031
00032 ROS_INFO_STREAM(ros::this_node::getName() << "/compute_dissimilarity: server ready");
00033 ros::spin();
00034
00035 return 0;
00036 }