float GoToSelectedBall::getAngleDiff(){ float robot_odom_x, robot_odom_y; getRobotPositionInOdom(robot_odom_x, robot_odom_y); float goalAngle = getAngle(robot_odom_x, robot_odom_y, getCurrentPose().x, getCurrentPose().y); float robotAngle = getRobotAngleInOdom(); return fabs(goalAngle-robotAngle); }
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"); }