int main(int argc, char** argv) { ros::init(argc, argv, "robot_move"); Explore robot_explore(ros::this_node::getName()); costmap_ros = new costmap_2d::Costmap2DROS("local_costmap", robot_explore.tf_listener_); double infRad = costmap_ros->getInflationRadius(); ROS_INFO("infRad =%f",infRad); while (!robot_explore.ac_.waitForServer(ros::Duration(5.0))) { //ROS_INFO("Waiting for the move_base action server to come up"); } robot_explore.setExploreState(STOP); ros::Rate loop_rate(1); while (ros::ok()) { ros::spinOnce(); loop_rate.sleep(); if (!robot_explore.isActionServerActive()){ //ROS_INFO("Explore server action isn't active!"); continue; } else{ if(robot_explore.getExploreState() == STOP){ robot_explore.setExploreState(RANDOM_ROTATE); robot_explore.randomRotate(); ROS_INFO("Start Explore STOP --> RANDOM_ROTATE"); } else if(robot_explore.getExploreState() == RANDOM_ROTATE){ if(robot_explore.isCurrentGoalDone()){ robot_explore.setExploreState(MAX_FORWARD); robot_explore.maxForward(); ROS_INFO("go to RANDOM_ROTATE --> MAX_FORWARD"); } } else if(robot_explore.getExploreState() == MAX_FORWARD){ if(robot_explore.isCurrentGoalDone()){ robot_explore.setExploreState(RANDOM_ROTATE); robot_explore.randomRotate(); ROS_INFO("go to MAX_FORWARD --> RANDOM_ROTATE"); } } } } return 0; }
int main(int argc, char** argv) { ros::init(argc, argv, "robot_move"); Explore robot_explore(ros::this_node::getName()); // ros::Subscriber obsMapSub = nh_.subscribe("/move_base/local_costmap/inflated_obstacles", 1, obstaclesMapCallback); costmap_ros = new costmap_2d::Costmap2DROS("local_costmap", robot_explore.tf_listener_); //costmap_ros->getCostmapCopy(costmap); double infRad = costmap_ros->getInflationRadius(); ROS_INFO("infRad =%f",infRad); ros::Rate loop_rate(1); // float rand_theta; // double x_odom, y_odom, rand_x, rand_y, x_map, y_map; while (!robot_explore.ac_.waitForServer(ros::Duration(5.0))) { ROS_INFO("Waiting for the move_base action server to come up"); } while (ros::ok()) { ros::spinOnce(); loop_rate.sleep(); /* if(robot_explore.canExplore() == false){ // ROS_INFO("Robot move - no explore - start ..."); // robot_explore.stopExplore(); // ros::Duration(1.0).sleep(); // ROS_INFO("Robot move - no explore - stop"); continue; }*/ if (!robot_explore.isActionServerActive()){ continue; } else{ costmap_ros->getCostmapCopy(costmap); if(robot_explore.getExploreState() == STOP){ // if(robot_explore.canExplore()){ robot_explore.setExploreState(FULL_ROTATE); robot_explore.robotFullRotate(); ROS_INFO("Start Explore STOP --> FULL_ROTATE"); // } } else if(robot_explore.getExploreState() == FULL_ROTATE){ if(robot_explore.isCurrentGoalDone()){ robot_explore.setExploreState(RANDOM_ROTATE); robot_explore.randomRotate(); ROS_INFO("go to FULL_ROTATE --> RANDOM_ROTATE"); } } else if(robot_explore.getExploreState() == RANDOM_ROTATE){ if(robot_explore.isCurrentGoalDone()){ robot_explore.setExploreState(MAX_FORWARD); robot_explore.maxForward(); ROS_INFO("go to RANDOM_ROTATE --> MAX_FORWARD"); } } else if(robot_explore.getExploreState() == MAX_FORWARD){ if(robot_explore.isCurrentGoalDone()){ robot_explore.setExploreState(RANDOM_ROTATE); robot_explore.randomRotate(); ROS_INFO("go to MAX_FORWARD --> RANDOM_ROTATE"); } } } /* robot_explore.robotFullRotate(); while(explore == true){ robot_explore.randomRotate(); std::cout<<"state randomRotate = "<<robot_explore.ac_.getState().toString()<<std::endl; if (robot_explore.ac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { ROS_INFO("Rotate Hooray, the base moved"); } else { ROS_INFO("Rotate The base failed to move for some reason"); } //robotMove.randomForward(); robot_explore.maxForward(); std::cout<<"state maxForward = "<<robot_explore.ac_.getState().toString()<<std::endl; if (robot_explore.ac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { ROS_INFO("Forward Hooray, the base moved"); } else { ROS_INFO("Forward The base failed to move for some reason"); } // std::cout<<"1 explore = "<<explore<<std::endl; ros::spinOnce(); loop_rate.sleep(); // std::cout<<"2 explore = "<<explore<<std::endl; } */ } return 0; }