void GoToSelectedBall::publishPose(float dist_from_ball){ ROS_INFO("enter publishPose, distance = %f", dist_from_ball); if(isBallPoseSet== false){ ROS_INFO("publishPose, wait for ball position, return"); return; } float ball_odom_x = getCurrentPose().x; float ball_odom_y = getCurrentPose().y; float robot_odom_x, robot_odom_y; getRobotPositionInOdom(robot_odom_x, robot_odom_y); float dx = ball_odom_x - robot_odom_x; float dy = ball_odom_y - robot_odom_y; float dist = sqrt(pow(ball_odom_x - robot_odom_x, 2) + pow(ball_odom_y-robot_odom_y, 2) ); ROS_INFO("dist = %f",dist); float goal_odom_x = robot_odom_x + dx * (dist - dist_from_ball) / dist; float goal_odom_y = robot_odom_y + dy * (dist - dist_from_ball) / dist; float angle = getAngle(robot_odom_x, robot_odom_y, ball_odom_x, ball_odom_y); tf::Quaternion q; float goal_map_x, goal_map_y; transFromOdomToMapPosition(goal_odom_x, goal_odom_y, angle, goal_map_x, goal_map_y, q); geometry_msgs::Quaternion qMsg; tf::quaternionTFToMsg(q, qMsg); move_base_msgs::MoveBaseGoal goal; goal.target_pose.pose.position.x = goal_map_x; goal.target_pose.pose.position.y = goal_map_y; goal.target_pose.pose.position.z = 0; goal.target_pose.pose.orientation = qMsg; goal.target_pose.header.stamp = ros::Time::now(); goal.target_pose.header.frame_id ="/map"; ROS_INFO("Sending goal"); ac.sendGoal(goal); firstGoalSent = true; }
void Explore::randomRotate(){ ROS_INFO("enter randomRotate "); float robot_odom_x, robot_odom_y; getRobotPositionInOdom(robot_odom_x, robot_odom_y); float angle = (((rand() % 360) - 180) * PI) / 180.0; tf::Quaternion q; float goal_map_x, goal_map_y; transFromOdomToMapPosition(robot_odom_x, robot_odom_y, angle, goal_map_x, goal_map_y, q); geometry_msgs::Quaternion qMsg; tf::quaternionTFToMsg(q, qMsg); move_base_msgs::MoveBaseGoal goal; goal.target_pose.pose.position.x = goal_map_x; goal.target_pose.pose.position.y = goal_map_y; goal.target_pose.pose.position.z = 0; goal.target_pose.pose.orientation = qMsg; goal.target_pose.header.stamp = ros::Time::now(); goal.target_pose.header.frame_id ="/map"; // ROS_INFO("Sending goal"); ac_.sendGoal(goal); // ROS_INFO("wait for result"); // ac_.waitForResult(); ROS_INFO("leave randomRotate"); }
void Explore::robotFullRotate(){ ROS_INFO("enter robotFullRotate "); float robot_odom_x, robot_odom_y; getRobotPositionInOdom(robot_odom_x, robot_odom_y); float angle_in_odom = getRobotAngleInOdom(); // kat od -PI do + PI float anglePlusPI = addPiToAngle(angle_in_odom); tf::Quaternion q; float goal_map_x, goal_map_y; transFromOdomToMapPosition(robot_odom_x, robot_odom_y, anglePlusPI, goal_map_x, goal_map_y, q); geometry_msgs::Quaternion qMsg; tf::quaternionTFToMsg(q, qMsg); move_base_msgs::MoveBaseGoal goal; goal.target_pose.pose.position.x = goal_map_x; goal.target_pose.pose.position.y = goal_map_y; goal.target_pose.pose.position.z = 0; goal.target_pose.pose.orientation = qMsg; goal.target_pose.header.stamp = ros::Time::now(); goal.target_pose.header.frame_id ="/map"; // ROS_INFO("Sending goal 1"); ac_.sendGoal(goal); /* ROS_INFO("wait for result"); ac_.waitForResult(); getRobotPositionInOdom(robot_odom_x, robot_odom_y); angle_in_odom = getRobotAngleInOdom(); // kat od -PI do + PI anglePlusPI = addPiToAngle(angle_in_odom); transFromOdomToMapPosition(robot_odom_x, robot_odom_y, anglePlusPI, goal_map_x, goal_map_y, q); tf::quaternionTFToMsg(q, qMsg); goal.target_pose.pose.position.x = goal_map_x; goal.target_pose.pose.position.y = goal_map_y; goal.target_pose.pose.position.z = 0; goal.target_pose.pose.orientation = qMsg; goal.target_pose.header.stamp = ros::Time::now(); goal.target_pose.header.frame_id ="/map"; ROS_INFO("Sending goal 2"); ac_.sendGoal(goal); ROS_INFO("wait for result"); ac_.waitForResult(); */ ROS_INFO("leave robotFullRotate"); }