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;
}
