void Explore::randomForward(){
ROS_INFO("enter randomForward ");
int counter = 0;
float rand_x, rand_y, x_odom, y_odom, x_map, y_map;
do {
rand_x = (rand() % 10) / 20.0;
rand_y = 0;
transfromRobotToOdomPosition(rand_x, rand_y, x_odom, y_odom);
++counter;
if(counter > 10){
ROS_INFO("leave randomForward, can not move forward");
return;
}
} while (!canMove(x_odom, y_odom));
float robotAngleInMap = getRobotAngleInMap();
transfromRobotToMapPosition(rand_x, rand_y, x_map, y_map);
// ROS_INFO("robot move to (%f,%f)", x_map, y_map);
publishPose(x_map, y_map, robotAngleInMap);
// ROS_INFO("wait for result");
// ac_.waitForResult();
ROS_INFO("leave randomForward");
}
void Explore::maxForward(){
ROS_INFO("enter maxForward ");
int counter = 0;
float d_x = 0, d_y=0, x_map, y_map, x_odom, y_odom;
transfromRobotToOdomPosition(d_x, d_y, x_odom, y_odom);
while (canMove(x_odom, y_odom)){
d_x += 0.1;
d_y = 0;
transfromRobotToOdomPosition(d_x, d_y, x_odom, y_odom);
/* ++counter;
if(counter > 10){
ROS_INFO("leave maxForward, can not move forward");
return;
}*/
};
// ROS_INFO("d_x = %f", d_x);
float robotAngleInMap = getRobotAngleInMap();
transfromRobotToMapPosition(d_x, d_y, x_map, y_map);
// ROS_INFO("robot move to (%f,%f)", x_map, y_map);
publishPose(x_map, y_map, robotAngleInMap);
// ROS_INFO("wait for result");
// ac_.waitForResult();
ROS_INFO("leave maxForward");
}
void Explore::maxForward(){
float d_x = 0.1, d_y=0.0, x_map, y_map;
transfromRobotToMapPosition(d_x, d_y, x_map, y_map);
while (canMove(x_map, y_map)){
d_x += 0.1;
transfromRobotToMapPosition(d_x, d_y, x_map, y_map);
};
ROS_INFO("d_x = %f", d_x);
publishPose(x_map, y_map, get, getRobotAngleInMap(), USE_MAP);
}