Removed INFO statements and setup for MAV

src/frontier_search.cpp

@@ -14,7 +14,7 @@ FrontierSearch::FrontierSearch(nav_msgs::OccupancyGrid &map, PoseHandlers::PoseH
   std::vector< std::vector<int> > vectorial_map (width, std::vector<int>(height));
   int data_index = 0;
 
-  std::cout<<"Organizing map over a bidimensional grid-like array"<<std::endl;
+  // std::cout<<"Organizing map over a bidimensional grid-like array"<<std::endl;
     for(int i=0; i<width; i++){
       for(int j=0; j<height; j++){
         vectorial_map[i][j] = map_.data[data_index];
@@ -23,7 +23,7 @@ FrontierSearch::FrontierSearch(nav_msgs::OccupancyGrid &map, PoseHandlers::PoseH
       }
     }
 
-  std::cout<<"Selecting frontier cells"<<std::endl;
+  // std::cout<<"Selecting frontier cells"<<std::endl;
   std::vector< std::pair<int, int> > frontier_cells;
   for(int i=0; i<width; i++){
     for(int j=0; j<height; j++){
@@ -35,7 +35,7 @@ FrontierSearch::FrontierSearch(nav_msgs::OccupancyGrid &map, PoseHandlers::PoseH
     }
   }
 
-  std::cout<<"assignement of frontier cells to groups-frontiers"<<std::endl;
+  // std::cout<<"assignement of frontier cells to groups-frontiers"<<std::endl;
   std::vector<std::vector<std::pair<int, int> > > frontiers;
   for(std::pair<int, int> coords : frontier_cells){
 
@@ -56,7 +56,7 @@ FrontierSearch::FrontierSearch(nav_msgs::OccupancyGrid &map, PoseHandlers::PoseH
       }
   }
 
-  std::cout<<"Frontiers at step 1 (detection): "<<frontiers.size()<<std::endl;
+  // std::cout<<"Frontiers at step 1 (detection): "<<frontiers.size()<<std::endl;
 
 
   for(std::vector<std::vector<std::pair<int, int> > >::iterator it_frontiers = frontiers.begin(); it_frontiers<(frontiers.end()-1);){
@@ -80,7 +80,7 @@ FrontierSearch::FrontierSearch(nav_msgs::OccupancyGrid &map, PoseHandlers::PoseH
     it_frontiers++;
    }
 
-  std::cout<<"Frontiers at step 2 (merging): "<<frontiers.size()<<std::endl;
+  // std::cout<<"Frontiers at step 2 (merging): "<<frontiers.size()<<std::endl;
 
 
 
@@ -91,7 +91,7 @@ FrontierSearch::FrontierSearch(nav_msgs::OccupancyGrid &map, PoseHandlers::PoseH
       i--;
     }
   }
-  std::cout<<"Frontiers at step 3 (filtering): "<<frontiers.size()<<std::endl;
+  // std::cout<<"Frontiers at step 3 (filtering): "<<frontiers.size()<<std::endl;
 
 
   return frontiers;
@@ -148,7 +148,7 @@ bool FrontierSearch::isOnSameFrontier(std::vector<std::pair<int, int> > frontier
  */
 std::vector<std::pair<int, int> > FrontierSearch::getCentroids(std::vector<std::vector<std::pair<int, int> > > frontiers){
 
-  tf::Transform transform = pose_handler_.lookupPose("/map", "/base_link");
+  tf::Transform transform = pose_handler_.lookupPose("/map", "/imu");
   float pose_x = transform.getOrigin().getX();
   float pose_y = transform.getOrigin().getY();
 //  map_pose.first  = (pose_x/map_.info.resolution) + (map_.info.height/2);
@@ -180,7 +180,7 @@ std::vector<std::pair<int, int> > FrontierSearch::getCentroids(std::vector<std::
   }
 
   std::swap(centroids[0], centroids[min_index]);
-  ROS_INFO("Closest centroid (x,y) map: %d, %d \t (x,y) rw pose: %f, %f  ", centroids[0].first, centroids[0].second, pose_x, pose_y);
+  // ROS_INFO("Closest centroid (x,y) map: %d, %d \t (x,y) rw pose: %f, %f  ", centroids[0].first, centroids[0].second, pose_x, pose_y);
 
 
   return centroids;

src/robot_exploration.cpp

@@ -40,7 +40,7 @@ int main(int argc, char **argv)
   ros::Publisher frontier_pub = robot_exploration.nh.advertise<nav_msgs::OccupancyGrid>("frontiers", 10);
   ros::Publisher goal_pub = robot_exploration.nh.advertise<geometry_msgs::PoseStamped>("/frontier_goal", 1);
 
-  ros::Rate loop_rate(10.0);
+  ros::Rate loop_rate(3.0);
 
   while(ros::ok()){
 
@@ -77,6 +77,7 @@ int main(int argc, char **argv)
     }
 
     frontier_pub.publish(robot_exploration.frontiers_map);
+    ros::spinOnce();
 
     if(!robot_exploration.centroids.empty()){
     geometry_msgs::PoseStamped goal;
@@ -86,12 +87,11 @@ int main(int argc, char **argv)
     goal.pose.position.y = (robot_exploration.centroids[0].first*robot_exploration.map.info.resolution)+robot_exploration.map.info.origin.position.y;
     goal.pose.position.z = 0;
     //ROS_INFO("%d, %f, %f", robot_exploration.centroids[0].first, robot_exploration.map.info.resolution, robot_exploration.map.info.origin.position.x);
-    ROS_INFO("Centroid (x,y) map: %d, %d \t (x,y) rw: %f, %f  ", robot_exploration.centroids[0].first, robot_exploration.centroids[0].second, goal.pose.position.x, goal.pose.position.y);
+    // ROS_INFO("Centroid (x,y) map: %d, %d \t (x,y) rw: %f, %f  ", robot_exploration.centroids[0].first, robot_exploration.centroids[0].second, goal.pose.position.x, goal.pose.position.y);
 
     goal_pub.publish(goal);
     }
 
-    ros::spinOnce();
 
     loop_rate.sleep();
 

src/send_goal.cpp

@@ -22,14 +22,14 @@ void centroid_callback(const geometry_msgs::PoseStampedConstPtr &msg){
   goal.target_pose.pose.orientation.w = 1.0;
 
   ROS_INFO("Sending goal");
-  ac->sendGoal(goal);
+  // ac->sendGoal(goal);
 
-  ac->waitForResult();
+  // ac->waitForResult();
 
-  if(ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
-    ROS_INFO("Hooray, the base moved to the goal");
-  else
-    ROS_INFO("The base failed to move for some reason");
+  // if(ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
+  //   ROS_INFO("Hooray, the base moved to the goal");
+  // else
+  //   ROS_INFO("The base failed to move for some reason");
 
 }
 
@@ -38,7 +38,7 @@ int main(int argc, char** argv){
 
   ros::NodeHandle nh;
 
-  ac = new MoveBaseClient("move_base", true);
+  // ac = new MoveBaseClient("move_base", true);
 
   ros::Subscriber centroid_sub = nh.subscribe("/frontier_goal", 5, centroid_callback);