explore_server.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <actionlib/server/simple_action_server.h>
00003 #include <actionlib/client/simple_action_client.h>
00004 #include <costmap_2d/costmap_2d_ros.h>
00005 #include <costmap_2d/costmap_2d.h>
00006 
00007 #include <geometry_msgs/PolygonStamped.h>
00008 
00009 #include <frontier_exploration/ExploreTaskAction.h>
00010 #include <frontier_exploration/GetNextFrontier.h>
00011 #include <frontier_exploration/UpdateBoundaryPolygon.h>
00012 
00013 #include <tf/transform_listener.h>
00014 
00015 #include <move_base_msgs/MoveBaseAction.h>
00016 
00017 #include <frontier_exploration/geometry_tools.h>
00018 
00019 namespace frontier_exploration{
00020 
00025 class FrontierExplorationServer
00026 {
00027 
00028 public:
00029 
00034     FrontierExplorationServer(std::string name) :
00035         tf_listener_(ros::Duration(10.0)),
00036         private_nh_("~"),
00037         as_(nh_, name, boost::bind(&FrontierExplorationServer::executeCb, this, _1), false),
00038         move_client_("move_base",true),
00039         retry_(5)
00040     {
00041         private_nh_.param<double>("frequency", frequency_, 0.0);
00042         private_nh_.param<double>("goal_aliasing", goal_aliasing_, 0.1);
00043 
00044         explore_costmap_ros_ = boost::shared_ptr<costmap_2d::Costmap2DROS>(new costmap_2d::Costmap2DROS("explore_costmap", tf_listener_));
00045 
00046         as_.registerPreemptCallback(boost::bind(&FrontierExplorationServer::preemptCb, this));
00047         as_.start();
00048     }
00049 
00050 private:
00051 
00052     ros::NodeHandle nh_;
00053     ros::NodeHandle private_nh_;
00054     tf::TransformListener tf_listener_;
00055     actionlib::SimpleActionServer<frontier_exploration::ExploreTaskAction> as_;
00056 
00057     boost::shared_ptr<costmap_2d::Costmap2DROS> explore_costmap_ros_;
00058     double frequency_, goal_aliasing_;
00059     bool success_, moving_;
00060     int retry_;
00061 
00062     boost::mutex move_client_lock_;
00063     frontier_exploration::ExploreTaskFeedback feedback_;
00064     actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_client_;
00065     move_base_msgs::MoveBaseGoal move_client_goal_;
00066 
00072     void executeCb(const frontier_exploration::ExploreTaskGoalConstPtr &goal)
00073     {
00074 
00075         success_ = false;
00076         moving_ = false;
00077 
00078         explore_costmap_ros_->resetLayers();
00079 
00080         //create costmap services
00081         ros::ServiceClient updateBoundaryPolygon = private_nh_.serviceClient<frontier_exploration::UpdateBoundaryPolygon>("explore_costmap/explore_boundary/update_boundary_polygon");
00082         ros::ServiceClient getNextFrontier = private_nh_.serviceClient<frontier_exploration::GetNextFrontier>("explore_costmap/explore_boundary/get_next_frontier");
00083 
00084         //wait for move_base and costmap services
00085         if(!move_client_.waitForServer() || !updateBoundaryPolygon.waitForExistence() || !getNextFrontier.waitForExistence()){
00086             as_.setAborted();
00087             return;
00088         }
00089 
00090         //set region boundary on costmap
00091         if(ros::ok() && as_.isActive()){
00092             frontier_exploration::UpdateBoundaryPolygon srv;
00093             srv.request.explore_boundary = goal->explore_boundary;
00094             if(updateBoundaryPolygon.call(srv)){
00095                 ROS_INFO("Region boundary set");
00096             }else{
00097                 ROS_ERROR("Failed to set region boundary");
00098                 as_.setAborted();
00099                 return;
00100             }
00101         }
00102 
00103         //loop until all frontiers are explored
00104         ros::Rate rate(frequency_);
00105         while(ros::ok() && as_.isActive()){
00106 
00107             frontier_exploration::GetNextFrontier srv;
00108 
00109             //placeholder for next goal to be sent to move base
00110             geometry_msgs::PoseStamped goal_pose;
00111 
00112             //get current robot pose in frame of exploration boundary
00113             tf::Stamped<tf::Pose> robot_pose;
00114             explore_costmap_ros_->getRobotPose(robot_pose);
00115 
00116             //provide current robot pose to the frontier search service request
00117             tf::poseStampedTFToMsg(robot_pose,srv.request.start_pose);
00118 
00119             //evaluate if robot is within exploration boundary using robot_pose in boundary frame
00120             geometry_msgs::PoseStamped eval_pose = srv.request.start_pose;
00121             if(eval_pose.header.frame_id != goal->explore_boundary.header.frame_id){
00122                 tf_listener_.transformPose(goal->explore_boundary.header.frame_id, srv.request.start_pose, eval_pose);
00123             }
00124 
00125             //check if robot is not within exploration boundary and needs to return to center of search area
00126             if(goal->explore_boundary.polygon.points.size() > 0 && !pointInPolygon(eval_pose.pose.position,goal->explore_boundary.polygon)){
00127                 
00128                 //check if robot has explored at least one frontier, and promote debug message to warning
00129                 if(success_){
00130                     ROS_WARN("Robot left exploration boundary, returning to center");
00131                 }else{
00132                     ROS_DEBUG("Robot not initially in exploration boundary, traveling to center");
00133                 }
00134                 //get current robot position in frame of exploration center
00135                 geometry_msgs::PointStamped eval_point;
00136                 eval_point.header = eval_pose.header;
00137                 eval_point.point = eval_pose.pose.position;
00138                 if(eval_point.header.frame_id != goal->explore_center.header.frame_id){
00139                     geometry_msgs::PointStamped temp = eval_point;
00140                     tf_listener_.transformPoint(goal->explore_center.header.frame_id, temp, eval_point);
00141                 }
00142 
00143                 //set goal pose to exploration center
00144                 goal_pose.header = goal->explore_center.header;
00145                 goal_pose.pose.position = goal->explore_center.point;
00146                 goal_pose.pose.orientation = tf::createQuaternionMsgFromYaw( yawOfVector(eval_point.point, goal->explore_center.point) );
00147 
00148             }else if(getNextFrontier.call(srv)){ //if in boundary, try to find next frontier to search
00149 
00150                 ROS_DEBUG("Found frontier to explore");
00151                 success_ = true;
00152                 goal_pose = feedback_.next_frontier = srv.response.next_frontier;
00153                 retry_ = 5;
00154 
00155             }else{ //if no frontier found, check if search is successful
00156                 ROS_DEBUG("Couldn't find a frontier");
00157 
00158                 //search is succesful
00159                 if(retry_ == 0 && success_){
00160                     ROS_WARN("Finished exploring room");
00161                     as_.setSucceeded();
00162                     boost::unique_lock<boost::mutex> lock(move_client_lock_);
00163                     move_client_.cancelGoalsAtAndBeforeTime(ros::Time::now());
00164                     return;
00165 
00166                 }else if(retry_ == 0 || !ros::ok()){ //search is not successful
00167 
00168                     ROS_ERROR("Failed exploration");
00169                     as_.setAborted();
00170                     return;
00171                 }
00172 
00173                 ROS_DEBUG("Retrying...");
00174                 retry_--;
00175                 //try to find frontier again, without moving robot
00176                 continue;
00177             }
00178             //if above conditional does not escape this loop step, search has a valid goal_pose
00179 
00180             //check if new goal is close to old goal, hence no need to resend
00181             if(!moving_ || !pointsNearby(move_client_goal_.target_pose.pose.position,goal_pose.pose.position,goal_aliasing_*0.5)){
00182                 ROS_DEBUG("New exploration goal");
00183                 move_client_goal_.target_pose = goal_pose;
00184                 boost::unique_lock<boost::mutex> lock(move_client_lock_);
00185                 if(as_.isActive()){
00186                     move_client_.sendGoal(move_client_goal_, boost::bind(&FrontierExplorationServer::doneMovingCb, this, _1, _2),0,boost::bind(&FrontierExplorationServer::feedbackMovingCb, this, _1));
00187                     moving_ = true;
00188                 }
00189                 lock.unlock();
00190             }
00191 
00192             //check if continuous goal updating is enabled
00193             if(frequency_ > 0){
00194                 //sleep for specified frequency and then continue searching
00195                 rate.sleep();
00196             }else{
00197                 //wait for movement to finish before continuing
00198                 while(ros::ok() && as_.isActive() && moving_){
00199                     ros::WallDuration(0.1).sleep();
00200                 }
00201             }
00202         }
00203 
00204         //goal should never be active at this point
00205         ROS_ASSERT(!as_.isActive());
00206 
00207     }
00208 
00209 
00213     void preemptCb(){
00214 
00215         boost::unique_lock<boost::mutex> lock(move_client_lock_);
00216         move_client_.cancelGoalsAtAndBeforeTime(ros::Time::now());
00217         ROS_WARN("Current exploration task cancelled");
00218 
00219         if(as_.isActive()){
00220             as_.setPreempted();
00221         }
00222 
00223     }
00224 
00229     void feedbackMovingCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback){
00230 
00231         feedback_.base_position = feedback->base_position;
00232         as_.publishFeedback(feedback_);
00233 
00234     }
00235 
00241     void doneMovingCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result){
00242 
00243         if (state == actionlib::SimpleClientGoalState::ABORTED){
00244             ROS_ERROR("Failed to move");
00245             as_.setAborted();
00246         }else if(state == actionlib::SimpleClientGoalState::SUCCEEDED){
00247             moving_ = false;
00248         }
00249 
00250     }
00251 
00252 };
00253 
00254 }
00255 
00256 int main(int argc, char** argv)
00257 {
00258     ros::init(argc, argv, "explore_server");
00259 
00260     frontier_exploration::FrontierExplorationServer server(ros::this_node::getName());
00261     ros::spin();
00262     return 0;
00263 }


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