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::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 GoToSelectedBall::publishPose(float x, float y){
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.pose.position.x = x;
goal.target_pose.pose.position.y = y;
geometry_msgs::Quaternion qMsg;
setAngle(getRobotAngleInMap(), qMsg);
goal.target_pose.pose.orientation = qMsg;
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.header.frame_id ="/odom";
ROS_INFO("Sending goal...");
ac.sendGoal(goal);
firstGoalSent = true;
}
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);
}
void GoToSelectedBall::goToBall(){
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.pose.position.x = getCurrentPose().x;
goal.target_pose.pose.position.y = getCurrentPose().y;
geometry_msgs::Quaternion qMsg;
setAngle(getRobotAngleInMap(), qMsg);
goal.target_pose.pose.orientation = qMsg;
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.header.frame_id ="/odom";
ROS_INFO("Sending goal...");
ac.sendGoalAndWait(goal,ros::Duration(120),ros::Duration(0.1));
firstGoalSent = true;
}
