distance_measure.cpp
Go to the documentation of this file.
00001 /* Copyright 2015 Institute of Digital Communication Systems - Ruhr-University Bochum
00002  * Author: Adrian Bauer
00003  *
00004  * This program is free software; you can redistribute it and/or modify it under the terms of
00005  * the GNU General Public License as published by the Free Software Foundation;
00006  * either version 3 of the License, or (at your option) any later version.
00007  * This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
00008  * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00009  * See the GNU General Public License for more details.
00010  * You should have received a copy of the GNU General Public License along with this program;
00011  * if not, see <http://www.gnu.org/licenses/>.
00012  *
00013  **/
00014 
00015 #include <ros/ros.h>
00016 #include <heatmap/wifi_signal.h>
00017 #include <heatmap/geometry_tools.h>
00018 #include <tf/transform_listener.h>
00019 #include <visualization_msgs/Marker.h>
00020 #include <visualization_msgs/MarkerArray.h>
00021 #include <heatmap/interpolation_service.h>
00022 #include <heatmap/signal_service.h>
00023 #include <geometry_msgs/PolygonStamped.h>
00024 
00025 #include <vector>
00026 #include <math.h>
00027 
00028 namespace heatmap
00029 {
00034 class DistanceMeasure
00035 {
00036 private:
00037   heatmap::wifi_signal current_sig;
00038   ros::Publisher marker_pub, interpol_marker_pub;
00039   ros::Subscriber signal_sub;
00040   int sig_count, av_sig;
00041   geometry_msgs::PolygonStamped polygon;
00042   std::vector<tf::Vector3> measure_list;
00043   std::vector<int> data_list;
00044   int marker_id;
00045   ros::NodeHandle n;
00046   int quality_max;
00047 
00053   double convertSignalToColor(heatmap::wifi_signal& signal)
00054   {
00055     double color;
00056 
00057     color = (((double)signal.link_quality - 35.0) * 2.0) / (double)signal.link_quality_max;
00058     if (color < 0)
00059       color = 0;
00060 
00061     return color;
00062   }
00063 
00071   visualization_msgs::Marker prepareSphereMarker(double color_g, double sphere_size, tf::Vector3& pos)
00072   {
00073     visualization_msgs::Marker marker;
00074 
00075     marker.header.frame_id = "/map";
00076     marker.header.stamp = ros::Time::now();
00077     marker.ns = "wifi_heatmap";
00078     marker.id = marker_id++;
00079     marker.type = visualization_msgs::Marker::SPHERE;
00080 
00081     // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
00082     marker.action = visualization_msgs::Marker::ADD;
00083 
00084     // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
00085     marker.pose.position.x = pos.x();
00086     marker.pose.position.y = pos.y();
00087     marker.pose.position.z = pos.z();
00088     marker.pose.orientation.x = 0.0;
00089     marker.pose.orientation.y = 0.0;
00090     marker.pose.orientation.z = 0.0;
00091     marker.pose.orientation.w = 1.0;
00092 
00093     // Set the scale of the marker
00094     marker.scale.x = sphere_size;
00095     marker.scale.y = sphere_size;
00096     marker.scale.z = sphere_size;
00097 
00098     // Set the color -- be sure to set alpha to something non-zero!
00099     marker.color.r = 1.0 - color_g;
00100     marker.color.g = color_g;
00101     marker.color.b = 0.0f;
00102     marker.color.a = 1.0;
00103 
00104     marker.lifetime = ros::Duration();
00105 
00106     return marker;
00107   }
00108 
00118   std::vector<double> interpolateAndVisualize(std::vector<geometry_msgs::Point32>& measure_points,
00119                                               std::vector<int>& measure_data, geometry_msgs::Polygon& poly,
00120                                               double interpolation_spacing, double shepard_power)
00121   {
00122     std::vector<geometry_msgs::Point32> interpolation_points;
00123     std::vector<double> interpolation_data;
00124     std::vector<visualization_msgs::Marker> marker_list;
00125     visualization_msgs::Marker marker;
00126     visualization_msgs::MarkerArray marker_array;
00127     heatmap::wifi_signal interpol_signal;
00128     tf::Vector3 marker_pos;
00129 
00130     /* Fill the measure area polygon with points considering the spacing between them */
00131     interpolation_points = heatmap::fillPolygon<geometry_msgs::Point32>(poly, interpolation_spacing);
00132     /* Now interpolate at these points */
00133     interpolation_data = heatmap::shepardInterpolation(interpolation_points, measure_points, measure_data, shepard_power);
00134 
00135     interpol_signal.link_quality_max = quality_max;
00136 
00137     /* Iterate over the interpolated data and prepare a RViz marker arary for them */
00138     for (int i = 0; i < interpolation_data.size(); i++)
00139     {
00140       interpol_signal.link_quality = interpolation_data.at(i);
00141       marker_pos.setValue(interpolation_points.at(i).x, interpolation_points.at(i).y, 0);
00142       marker = prepareSphereMarker(convertSignalToColor(interpol_signal), interpolation_spacing, marker_pos);
00143       marker_list.push_back(marker);
00144     }
00145 
00146     /* Publish this marker array */
00147     marker_array.markers = marker_list;
00148     interpol_marker_pub.publish(marker_array);
00149     ROS_INFO("Interpolation done!");
00150 
00151     return interpolation_data;
00152   }
00153 
00154   bool startInterpolation(heatmap::interpolation_service::Request &req, heatmap::interpolation_service::Response &res)
00155   {
00156     std::vector<geometry_msgs::Point32> point_measure_list;
00157     geometry_msgs::Point32 p;
00158     /* Convert tf::Vector3 to std::vector */
00159     for (int i = 0; i < measure_list.size(); i++)
00160     {
00161       tf::Vector3 vect = measure_list.at(i);
00162       p.x = vect.getX();
00163       p.y = vect.getY();
00164       point_measure_list.push_back(p);
00165     }
00166 
00167     ROS_INFO("Interpolation called with spacing: %f and Shepard power: %f", (double )req.spacing,
00168              (double )req.shepard_power);
00169 
00170     std::vector<double> interpolated = interpolateAndVisualize(point_measure_list, data_list, polygon.polygon,
00171                                                                (double)req.spacing, (double)req.shepard_power);
00172 
00173     return true;
00174   }
00175 
00176   void polygonCallback(const geometry_msgs::PolygonStamped& poly)
00177   {
00178     polygon = poly;
00179   }
00180 
00181 
00182 public:
00183   DistanceMeasure() :
00184     n()
00185 {
00186     sig_count = 0;
00187     av_sig = 0;
00188     marker_id = 0;
00189     /* Distance in meters to take a new measurement */
00190     const double MEASURE_DISTANCE = 0.3;
00191     heatmap::signal_service srv;
00192     heatmap::wifi_signal signal;
00193     tf::Vector3 last_measure(0, 0, 0), last_transform;
00194     tfScalar dist;
00195 
00196     tf::TransformListener listener;
00197     tf::StampedTransform transform;
00198 
00199     visualization_msgs::Marker mark;
00200 
00201     ros::Rate rate(5.0);
00202     ros::Subscriber polygon_sub = n.subscribe("heatmap_area", 1, &DistanceMeasure::polygonCallback, this);
00203 
00204     marker_pub = n.advertise<visualization_msgs::Marker>("signal_marker", 0);
00205     interpol_marker_pub = n.advertise<visualization_msgs::MarkerArray>("interpol_marker", 0);
00206 
00207     /* Register a service client to get the signal strength from the signal node */
00208     ros::ServiceClient client = n.serviceClient<heatmap::signal_service>("get_wifi_signal");
00209     /* Register a service server to offer the interpolation feature */
00210     ros::ServiceServer service = n.advertiseService("start_interpol", &DistanceMeasure::startInterpolation, this);
00211     /* Loop until we receive a shutdown request */
00212     while (n.ok())
00213     {
00214       /* Starting condition: Let's take a new measurement
00215        * Might be set to false later inside the loop
00216        */
00217       bool measure = true;
00218       try
00219       {
00220         /* Wait for a new transform from /map to base_footprint, then look it up */
00221         // ROS_INFO("Waiting for a transform from /map to base_footprint...");
00222         listener.waitForTransform("/map", "base_footprint", ros::Time(0), ros::Duration(10.0));
00223         listener.lookupTransform("/map", "base_footprint", ros::Time(0), transform);
00224         last_measure = transform.getOrigin();
00225 
00226         /* Iterate over each measurement vector */
00227         for (std::vector<tf::Vector3>::iterator it = measure_list.begin(); it != measure_list.end(); ++it)
00228         {
00229           /* Get the distance from the current transform (position) to the vector */
00230           dist = last_measure.distance(*it);
00231           /* If any distance exists which is lower than our limit
00232            * Don't measure this time and leave the iteration
00233            */
00234           if (dist < MEASURE_DISTANCE)
00235           {
00236             measure = false;
00237             break;
00238           }
00239         }
00240 
00241         if (measure)
00242         {
00243           /* Get the WIFI signal quality via service call */
00244           if (client.call(srv))
00245           {
00246             signal = srv.response.signal;
00247             /* save the maximum quality. It shouldn't change. */
00248             quality_max = signal.link_quality_max;
00249             ROS_INFO("Distance for a new measurement reached at: x=%f, y=%f", last_measure.getX(), last_measure.getY());
00250             /* Prepare and publish a RViz marker to display the measured WIFI signal */
00251             mark = prepareSphereMarker(convertSignalToColor(signal), MEASURE_DISTANCE, last_measure);
00252             marker_pub.publish(mark);
00253             /* Save the coordinates and corresponding signal quality for interpolation */
00254             measure_list.push_back(last_measure);
00255             data_list.push_back(signal.link_quality);
00256           }
00257           else
00258           {
00259             ROS_ERROR("get_wifi_signal service call failed!");
00260           }
00261         }
00262       }
00263       catch (tf::TransformException ex)
00264       {
00265         ROS_ERROR("%s", ex.what());
00266       }
00267       ros::spinOnce();
00268       rate.sleep();
00269     }
00270 }
00271 };
00272 
00273 }
00274 
00275 int main(int argc, char** argv)
00276 {
00277   ros::init(argc, argv, "distance_measure");
00278 
00279   heatmap::DistanceMeasure dm;
00280 
00281   return 0;
00282 }


heatmap
Author(s): Adrian Bauer
autogenerated on Thu Feb 11 2016 23:05:26