explore_client.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <actionlib/client/simple_action_client.h>
00003 #include <costmap_2d/costmap_2d_ros.h>
00004 #include <costmap_2d/costmap_2d.h>
00005 
00006 #include <geometry_msgs/PolygonStamped.h>
00007 #include <geometry_msgs/PointStamped.h>
00008 
00009 #include <frontier_exploration/geometry_tools.h>
00010 
00011 #include <frontier_exploration/ExploreTaskAction.h>
00012 #include <frontier_exploration/ExploreTaskActionGoal.h>
00013 #include <frontier_exploration/GetNextFrontier.h>
00014 #include <frontier_exploration/UpdateBoundaryPolygon.h>
00015 #include <costmap_2d/footprint.h>
00016 #include <tf/transform_listener.h>
00017 
00018 #include <ros/wall_timer.h>
00019 
00020 #include <move_base_msgs/MoveBaseAction.h>
00021 
00022 #include <visualization_msgs/Marker.h>
00023 #include <boost/foreach.hpp>
00024 
00025 
00026 namespace frontier_exploration{
00027 
00031 class FrontierExplorationClient{
00032 
00033 private:
00034 
00035     ros::NodeHandle nh_;
00036     ros::NodeHandle private_nh_;
00037 
00038     ros::Subscriber point_;
00039     ros::Publisher point_viz_pub_;
00040     ros::WallTimer point_viz_timer_;
00041     geometry_msgs::PolygonStamped input_;
00042 
00043     bool waiting_for_center_;
00044 
00048     void vizPubCb(){
00049 
00050         visualization_msgs::Marker points, line_strip;
00051 
00052         points.header = line_strip.header = input_.header;
00053         points.ns = line_strip.ns = "explore_points";
00054 
00055         points.id = 0;
00056         line_strip.id = 1;
00057 
00058         points.type = visualization_msgs::Marker::SPHERE_LIST;
00059         line_strip.type = visualization_msgs::Marker::LINE_STRIP;
00060 
00061         if(!input_.polygon.points.empty()){
00062 
00063             points.action = line_strip.action = visualization_msgs::Marker::ADD;
00064             points.pose.orientation.w = line_strip.pose.orientation.w = 1.0;
00065 
00066             points.scale.x = points.scale.y = 0.1;
00067             line_strip.scale.x = 0.05;
00068 
00069             BOOST_FOREACH(geometry_msgs::Point32 point, input_.polygon.points){
00070                 line_strip.points.push_back(costmap_2d::toPoint(point));
00071                 points.points.push_back(costmap_2d::toPoint(point));
00072             }
00073 
00074             if(waiting_for_center_){
00075                 line_strip.points.push_back(costmap_2d::toPoint(input_.polygon.points.front()));
00076                 points.color.a = points.color.r = line_strip.color.r = line_strip.color.a = 1.0;
00077             }else{
00078                 points.color.a = points.color.b = line_strip.color.b = line_strip.color.a = 1.0;
00079             }
00080         }else{
00081             points.action = line_strip.action = visualization_msgs::Marker::DELETE;
00082         }
00083         point_viz_pub_.publish(points);
00084         point_viz_pub_.publish(line_strip);
00085 
00086     }
00087 
00092     void pointCb(const geometry_msgs::PointStampedConstPtr& point){
00093       
00094         double average_distance = polygonPerimeter(input_.polygon) / input_.polygon.points.size();
00095 
00096         if(waiting_for_center_){
00097             //flag is set so this is the last point of boundary polygon, i.e. center
00098 
00099             if(!pointInPolygon(point->point,input_.polygon)){
00100                 ROS_ERROR("Center not inside polygon, restarting");
00101             }else{
00102                 actionlib::SimpleActionClient<frontier_exploration::ExploreTaskAction> exploreClient("explore_server", true);
00103                 exploreClient.waitForServer();
00104                 ROS_INFO("Sending goal");
00105                 frontier_exploration::ExploreTaskGoal goal;
00106                 goal.explore_center = *point;
00107                 goal.explore_boundary = input_;
00108                 exploreClient.sendGoal(goal);
00109             }
00110             waiting_for_center_ = false;
00111             input_.polygon.points.clear();
00112 
00113         }else if(input_.polygon.points.empty()){
00114             //first control point, so initialize header of boundary polygon
00115 
00116             input_.header = point->header;
00117             input_.polygon.points.push_back(costmap_2d::toPoint32(point->point));
00118 
00119         }else if(input_.header.frame_id != point->header.frame_id){
00120             ROS_ERROR("Frame mismatch, restarting polygon selection");
00121             input_.polygon.points.clear();
00122 
00123         }else if(input_.polygon.points.size() > 1 && pointsNearby(input_.polygon.points.front(), point->point,
00124                                                                     average_distance*0.1)){
00125             //check if last boundary point, i.e. nearby to first point
00126 
00127             if(input_.polygon.points.size() < 3){
00128                 ROS_ERROR("Not a valid polygon, restarting");
00129                 input_.polygon.points.clear();
00130             }else{
00131                 waiting_for_center_ = true;
00132                 ROS_WARN("Please select an initial point for exploration inside the polygon");
00133             }
00134 
00135         }else{
00136 
00137             //otherwise, must be a regular point inside boundary polygon
00138             input_.polygon.points.push_back(costmap_2d::toPoint32(point->point));
00139             input_.header.stamp = ros::Time::now();
00140         }
00141 
00142     }
00143 
00144 public:
00145 
00149     FrontierExplorationClient() :
00150         nh_(),
00151         private_nh_("~"),
00152         waiting_for_center_(false)
00153     {
00154         input_.header.frame_id = "map";
00155         point_ = nh_.subscribe("/clicked_point",10,&FrontierExplorationClient::pointCb, this);
00156         point_viz_pub_ = nh_.advertise<visualization_msgs::Marker>("exploration_polygon_marker", 10);
00157         point_viz_timer_ = nh_.createWallTimer(ros::WallDuration(0.1), boost::bind(&FrontierExplorationClient::vizPubCb, this));
00158         ROS_INFO("Please use the 'Point' tool in Rviz to select an exporation boundary.");
00159     }    
00160 
00161 };
00162 
00163 }
00164 
00165 int main(int argc, char** argv)
00166 {
00167     ros::init(argc, argv, "explore_client");
00168 
00169     frontier_exploration::FrontierExplorationClient client;
00170     ros::spin();
00171     return 0;
00172 }


frontier_exploration
Author(s): Paul Bovbel
autogenerated on Fri Aug 28 2015 10:43:32