heatmap_client.cpp
Go to the documentation of this file.
00001 /* Original work Copyright Paul Bovbel
00002  * Modified work Copyright 2015 Institute of Digital Communication Systems - Ruhr-University Bochum
00003  * Modified by: Adrian Bauer
00004  *
00005  * This program is free software; you can redistribute it and/or modify it under the terms of
00006  * the GNU General Public License as published by the Free Software Foundation;
00007  * either version 3 of the License, or (at your option) any later version.
00008  * This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
00009  * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00010  * See the GNU General Public License for more details.
00011  * You should have received a copy of the GNU General Public License along with this program;
00012  * if not, see <http://www.gnu.org/licenses/>.
00013  *
00014  * This file is a modified version of the original file taken from the frontier_exploration ROS package by Paul Bovbel
00015  **/
00016 
00017 #include <ros/ros.h>
00018 #include <costmap_2d/costmap_2d_ros.h>
00019 
00020 #include <geometry_msgs/PolygonStamped.h>
00021 #include <geometry_msgs/PointStamped.h>
00022 
00023 #include <heatmap/geometry_tools.h>
00024 
00025 #include <ros/wall_timer.h>
00026 
00027 #include <visualization_msgs/Marker.h>
00028 
00029 namespace heatmap
00030 {
00031 
00035 class HeatmapClient
00036 {
00037 
00038 private:
00039 
00040   ros::NodeHandle nh_;
00041   ros::NodeHandle private_nh_;
00042 
00043   ros::Subscriber point_;
00044   ros::Publisher point_viz_pub_, polygon_planner_pub;
00045   ros::WallTimer point_viz_timer_;
00046   geometry_msgs::PolygonStamped input_;
00047 
00051   void vizPubCb()
00052   {
00053 
00054     visualization_msgs::Marker points, line_strip;
00055 
00056     points.header = line_strip.header = input_.header;
00057     points.ns = line_strip.ns = "explore_points";
00058 
00059     points.id = 0;
00060     line_strip.id = 1;
00061 
00062     points.type = visualization_msgs::Marker::SPHERE_LIST;
00063     line_strip.type = visualization_msgs::Marker::LINE_STRIP;
00064 
00065     if (!input_.polygon.points.empty())
00066     {
00067       points.action = line_strip.action = visualization_msgs::Marker::ADD;
00068       points.pose.orientation.w = line_strip.pose.orientation.w = 1.0;
00069 
00070       points.scale.x = points.scale.y = 0.1;
00071       line_strip.scale.x = 0.05;
00072 
00073       for(int i = 0; i < input_.polygon.points.size(); i ++)
00074       {
00075         geometry_msgs::Point32 point = input_.polygon.points.at(i);
00076         line_strip.points.push_back(costmap_2d::toPoint(point));
00077         points.points.push_back(costmap_2d::toPoint(point));
00078       }
00079 
00080     points.color.a = points.color.b = line_strip.color.b = line_strip.color.a = 1.0;
00081 
00082     }
00083     else
00084     {
00085       points.action = line_strip.action = visualization_msgs::Marker::DELETE;
00086     }
00087 
00088     point_viz_pub_.publish(points);
00089     point_viz_pub_.publish(line_strip);
00090   }
00091 
00096   void pointCb(const geometry_msgs::PointStampedConstPtr& point)
00097   {
00098 
00099     double average_distance = polygonPerimeter(input_.polygon) / input_.polygon.points.size();
00100 
00101     if (input_.polygon.points.empty())
00102     {
00103       //first control point, so initialize header of boundary polygon
00104 
00105       input_.header = point->header;
00106       input_.polygon.points.push_back(costmap_2d::toPoint32(point->point));
00107 
00108     }
00109     else if (input_.header.frame_id != point->header.frame_id)
00110     {
00111       ROS_ERROR("Frame mismatch, restarting polygon selection");
00112       input_.polygon.points.clear();
00113 
00114     }
00115     else if (input_.polygon.points.size() > 1
00116         && pointsNearby(input_.polygon.points.front(), point->point, average_distance * 0.1))
00117     {
00118       //check if last boundary point, i.e. nearby to first point
00119 
00120       if (input_.polygon.points.size() < 3)
00121         ROS_ERROR("Not a valid polygon, restarting");
00122       else
00123         polygon_planner_pub.publish(input_);
00124 
00125       input_.polygon.points.clear();
00126     }
00127     else
00128     {
00129       //otherwise, must be a regular point inside boundary polygon
00130       input_.polygon.points.push_back(costmap_2d::toPoint32(point->point));
00131       input_.header.stamp = ros::Time::now();
00132     }
00133 
00134   }
00135 
00136 public:
00140   HeatmapClient() :
00141       nh_(), private_nh_("~")
00142   {
00143     input_.header.frame_id = "map";
00144     point_ = nh_.subscribe("/clicked_point", 10, &HeatmapClient::pointCb, this);
00145     polygon_planner_pub = nh_.advertise<geometry_msgs::PolygonStamped>("heatmap_area", 10);
00146     point_viz_pub_ = nh_.advertise<visualization_msgs::Marker>("heatmap_polygon_marker", 10);
00147     point_viz_timer_ = nh_.createWallTimer(ros::WallDuration(0.1),
00148                                            boost::bind(&HeatmapClient::vizPubCb, this));
00149     ROS_INFO("Please use the 'Point' tool in Rviz to select a heatmap boundary.");
00150   }
00151 
00152 };
00153 
00154 }
00155 
00156 int main(int argc, char** argv)
00157 {
00158   ros::init(argc, argv, "explore_client");
00159 
00160   heatmap::HeatmapClient client;
00161   ros::spin();
00162   return 0;
00163 }


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