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;
}
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 GoToSelectedBall::publishAngle(){
ROS_INFO("enter publishAngle");
if(isBallPoseSet== false){
ROS_INFO("publishAngle, 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 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);
setAngle(angle, qMsg);
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.pose.position.x = goal_odom_x;
goal.target_pose.pose.position.y = goal_odom_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 ="/odom";
ROS_INFO("Sending goal");
ac.sendGoal(goal);
firstGoalSent = true;
// isBallPoseSet = false;
ROS_INFO("return publishAngle");
}
float GoToSelectedBall::getDistanceFromSelectedBall(){
float robot_odom_x, robot_odom_y, ball_odom_x, ball_odom_y;
getRobotPositionInOdom(robot_odom_x, robot_odom_y);
ball_odom_x = current_pose_.x;
ball_odom_y = current_pose_.y;
// ROS_INFO("robot (x, y) i odom = (%f, %f)", robot_odom_x, robot_odom_y);
// ROS_INFO("ball (x, y) i odom = (%f, %f)", ball_odom_x, ball_odom_y);
float dist = sqrt(pow(ball_odom_x - robot_odom_x, 2) + pow(ball_odom_y-robot_odom_y, 2) );
return dist;
}
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");
}
void ChooseAccessibleBalls::allBallsCb(const geometry_msgs::PoseArrayConstPtr& all_balls_msg){
// ROS_INFO("enter ChooseOneBall");
/*
if(explore == true){
return;
}
*/
ros::Time start = ros::Time::now();
double robot_odom_x, robot_odom_y;
getRobotPositionInOdom(robot_odom_x, robot_odom_y);
// ROS_INFO("(robot_odom_x, robot_odom_y) (%f, %f)", robot_odom_x, robot_odom_y);
std::vector<geometry_msgs::Pose> allBalls = all_balls_msg->poses;
// ROS_INFO("number of balls = %d", (int)allBalls.size());
/* if(first != false){
return;
}*/
/*
if(allBalls.size() != 1){
return;
}
first = true;
*/
geometry_msgs::PoseArray accesibleBallsMsg;
accesibleBallsMsg.header.frame_id ="/odom";
std::vector<geometry_msgs::Pose> poses;
ros::Time past = all_balls_msg->header.stamp;
tf::StampedTransform tfOC; // kamera w odom
tf_listener_.waitForTransform("/odom", "/openni_rgb_optical_frame", past, ros::Duration(1.0));
tf_listener_.lookupTransform ("/odom", "/openni_rgb_optical_frame", past, tfOC);
for (unsigned int i = 0; i < allBalls.size(); i++){
double x = allBalls[i].position.x;
double y = allBalls[i].position.y;
double z = allBalls[i].position.z;
// ROS_INFO("(x, y) (%f, %f)", x, y);
double ball_odom_x, ball_odom_y, ball_odom_z;
transFromCameraToOdomPosition(x, y, z, ball_odom_x, ball_odom_y, ball_odom_z, tfOC);
if(ball_odom_z < 0.03 ||ball_odom_z > 0.05){
continue;
}
// ROS_INFO("(x_odom, y_odom) (%f, %f)", x_odom, y_odom);
float goal_odom_x, goal_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 dx = ball_odom_x - robot_odom_x;
float dy = ball_odom_y - robot_odom_y;
goal_odom_x = robot_odom_x + dx * (dist - .70) / dist;
goal_odom_y = robot_odom_y + dy * (dist - .70) / dist;
geometry_msgs::Pose pose;
pose.position.x = ball_odom_x;
pose.position.y = ball_odom_y;
pose.position.z = ball_odom_z;
pose.orientation = allBalls[i].orientation;
poses.push_back(pose);
bool canMove1 = canMove(goal_odom_x, goal_odom_y);
if(canMove1==true){
// ROS_INFO(" can move to ball at (%f, %f)", goal_odom_x, goal_odom_y);
geometry_msgs::Pose pose;
pose.position.x = ball_odom_x;
pose.position.y = ball_odom_y;
pose.position.z = ball_odom_z;
pose.orientation = allBalls[i].orientation;
poses.push_back(pose);
}
else{
// ROS_INFO(" can not move to ball at (%f, %f)",goal_odom_x, goal_odom_y);
}
}
ros::Time finish = ros::Time::now();
ros::Duration duration = finish - start;
double durSec = duration.toNSec()/1000000;
double avgDur = durSec/allBalls.size();
ROS_INFO("dur = %f [ms]", durSec);
ROS_INFO("ball size = %d, average dur = %f [ms]", (int)allBalls.size(), avgDur);
// if(poses.size() > 0 ){
accesibleBallsMsg.poses = poses;
accesible_balls.publish(accesibleBallsMsg);
// }
}
