void GoToSelectedBall::executeCB(const scheduler::SchedulerGoalConstPtr &goal){
ROS_INFO("enter executeCB, goal = %i", goal->value);
if(goal->value == 0){
state_ = STOP;
}
else if(goal->value == 1){
state_ = FIRST_STEP_COLLECT;
}
else if(goal->value == 2){
// TODO: sprawdza, czy jest ustawiona pozycja pileczki, albo przesylac ja razem z goalem
// TODO: dopisac serwer do jazdy do przodu a nie na sleep tak jak teraz
state_ = SECOND_STEP_COLLECT;
goForward(0);
ROS_INFO("enter SECOND_STEP_COLLECT");
publishAngle();
ac.waitForResult();
float dist = getDistanceFromSelectedBall();
onHoover();
goForward(dist -0.3);
ros::Duration(4.0).sleep();
goForward(-(dist-0.3));
ros::Duration(5.0).sleep();
goForward(0);
offHoover();
ROS_INFO("leave SECOND_STEP_COLLECT");
}
as_.publishFeedback(feedback_);
result_.value = feedback_.value;
as_.setSucceeded(result_);
ROS_INFO("leave executeCB");
}
/*
Tries to move the robot to the next search place
*/
void robotGoToNextSearchPlace(MoveBaseClient &ac) {
if (nextSearchPlace >= 2){
ROS_INFO("Back to first search place");
nextSearchPlace = 0;
}
//create goal command
goalCmd.target_pose.header.frame_id = "map";
goalCmd.target_pose.header.stamp = ros::Time::now();
goalCmd.target_pose.pose.position.x = searchPlace[nextSearchPlace][0];
goalCmd.target_pose.pose.position.y = searchPlace[nextSearchPlace][1];
float angle = searchPlace[nextSearchPlace][2];
tf::Quaternion quaternion = tf::createQuaternionFromYaw(angle);
geometry_msgs::Quaternion qangle;
tf::quaternionTFToMsg(quaternion, qangle);
goalCmd.target_pose.pose.orientation = qangle;
// Send the goal
ac.sendGoal(goalCmd);
ROS_INFO("Starting to move to new search position");
waitUntilDoneCancelIfToyFound(ac);
if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
ROS_INFO("Hooray, the base have moved to search position %d!", nextSearchPlace);
}
else {
ROS_INFO("Did not actually go to search place, stopped trying");
}
}
/*
Spins the robot at its place one full round to search for toys
*/
void spinRobot(MoveBaseClient &ac) {
for (int i = 1; i < NUMBER_SPIN_ANGLES; i++) {
goalCmd.target_pose.header.frame_id = "base_link";
goalCmd.target_pose.header.stamp = ros::Time::now();
//robot only rotates, x and y are not changed according to base_link
goalCmd.target_pose.pose.position.x = 0;
goalCmd.target_pose.pose.position.y = 0;
//calculate angle into quaternion
float angle = -(2*PI / NUMBER_SPIN_ANGLES);
tf::Quaternion quaternion = tf::createQuaternionFromYaw(angle);
geometry_msgs::Quaternion qangle;
tf::quaternionTFToMsg(quaternion, qangle);
goalCmd.target_pose.pose.orientation = qangle;
// Send the goal
ac.sendGoal(goalCmd);
waitUntilDone(ac);
if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
ROS_INFO("Hooray, the base have turned %d / %d", (i),NUMBER_SPIN_ANGLES);
}
}
}
void MultiNavigation::run(nav_msgs::Path path) {
// Wait for the action server to become available
ROS_INFO("Waiting for the move_base action server");
ac->waitForServer(ros::Duration(5));
ROS_INFO("Connected to move base server");
for(int i = 0; i < path.poses.size(); i++) {
// Send a goal to move_base
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = path.poses[i].pose.position.x;
goal.target_pose.pose.position.y = path.poses[i].pose.position.y;
goal.target_pose.pose.orientation = path.poses[i].pose.orientation;
ac->sendGoal(goal);
// Wait for the action to return
ac->waitForResult();
if (ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("You have reached the goal!");
else
ROS_INFO("The base failed for some reason");
ros::spinOnce();
ros::Duration(0.5).sleep();
}
}
/*
Waits until goal is reached or cancelled, or when toy is found on the way
*/
void waitUntilDoneCancelIfToyFound(MoveBaseClient &ac) {
bool cancelled = false;
ROS_INFO("In waitUntilDoneCancelIfToyFound");
while (ros::ok() && !ac.waitForResult(ros::Duration(CHECK_INTERVAL_WAIT_FOR_RESULT))) {
if (!cancelled && isToyToPick()){
ROS_INFO("Toy in array, cancel movement");
ac.cancelGoal();
cancelled = true;
}
ros::spinOnce();
}
}
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::preemptCB(){
ROS_INFO("%s: Preempted", action_name_.c_str());
explore_ = false;
explore_state_ = STOP;
ac_.cancelAllGoals ();
as_.setPreempted();
}
/*
Waits until goal is reached or cancelled
*/
void waitUntilDone(MoveBaseClient &ac){
ROS_INFO("In waitUntilDone");
while(ros::ok() && !ac.waitForResult(ros::Duration(CHECK_INTERVAL_WAIT_FOR_RESULT))){
ros::spinOnce();
}
ROS_INFO("Leaving waitUntilDone");
}
void Explore::publishPose(float x, float y, float theta, bool robot){
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.pose.position.x = x;
goal.target_pose.pose.position.y = y;
geometry_msgs::Quaternion qMsg;
setAngle(theta, qMsg);
goal.target_pose.pose.orientation = qMsg;
goal.target_pose.header.stamp = ros::Time::now();
if(robot)
{
goal.target_pose.header.frame_id ="/base_link";
} else {
goal.target_pose.header.frame_id ="/map";
}
ROS_INFO("Sending goal...");
ac_.sendGoalAndWait(goal,ros::Duration(TIME_FOR_GOAL),ros::Duration(0.1) );
firstGoalSend = true;
}
void sendGoal(nav_msgs::Path& p, MoveBaseClient& ac){
move_base_msgs::MoveBaseGoal goal;
goal.target_pose =p.poses[p.poses.size()-1];
ROS_INFO("Sending goal for x:%lf / y:%lf",goal.target_pose.pose.position.x,goal.target_pose.pose.position.y);
ac.sendGoal(goal);
//ac.waitForResult();
}
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;
}
/*
Moves robot to given coordinates and angle
*/
bool robotGoToCoord(geometry_msgs::Point &coord, MoveBaseClient &ac, float angle/*=UP*/){
bool retVal = false;
goalCmd.target_pose.header.frame_id = "map";
goalCmd.target_pose.header.stamp = ros::Time::now();
goalCmd.target_pose.pose.position.x = coord.x;
goalCmd.target_pose.pose.position.y = coord.y;
tf::Quaternion quaternion = tf::createQuaternionFromYaw(angle);
geometry_msgs::Quaternion qangle;
tf::quaternionTFToMsg(quaternion, qangle);
goalCmd.target_pose.pose.orientation = qangle;
// Send the goal
ac.sendGoal(goalCmd);
waitUntilDone(ac);
if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
ROS_INFO("Hooray, the base has moved to given coordinate!");
retVal = true;
}
return retVal;
}
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");
}
void ChooseAccessibleBalls::publishPose(float x, float y, geometry_msgs::Quaternion qMsg){
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.pose.position.x = x;
goal.target_pose.pose.position.y = y;
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);
}
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::publishPose(float x, float y, float theta){
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.pose.position.x = x;
goal.target_pose.pose.position.y = y;
geometry_msgs::Quaternion qMsg;
setAngle(theta, qMsg);
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);
firstGoalSend = true;
}
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;
}
void Explore::stopExplore(){
ac_.cancelAllGoals ();
}
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 runPlanner()
{
while(ros::ok())
{
rate->sleep();
//If we are receiving data
switch(currentState)
{
case STOPPED:
//ROS_ERROR("STOPPED!");
//Next state
if(distanceToPerson > planner_activation_distance && hold == false)
{
nextState = PLANNER;
delete rate;
rate = new ros::Rate(10);
}
break;
case PLANNER:
ROS_ERROR("PLANNER");
if(distanceToPerson >= minimum_planner_distance && hold == false)
{
nextState = PLANNER;
//Get transforms
tf::StampedTransform transform;
try
{
listener.waitForTransform(world_frame, robot_frame, ros::Time(0), ros::Duration(10.0) );
listener.lookupTransform(world_frame, robot_frame,ros::Time(0), transform);
}
catch(tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
Eigen::Affine3d eigen_transform;
tf::transformTFToEigen(transform, eigen_transform);
// convert matrix from Eigen to openCV
cv::Mat baseLinkToMap;
cv::eigen2cv(eigen_transform.matrix(), baseLinkToMap);
cv::Point3d personPoint;
personPoint.x = personInRobotBaseFrame.point.x;
personPoint.y = personInRobotBaseFrame.point.y;
personPoint.z = personInRobotBaseFrame.point.z;
segwayController::moveBase(personPoint, baseLinkToMap, ac, distance_to_target);
}
else if(distanceToPerson < minimum_planner_distance || hold == true)
{
nextState = STOPPED;
ac->cancelAllGoals();
delete rate;
rate = new ros::Rate(10);
}
break;
default:
nextState = STOPPED;
ac->cancelAllGoals();
}
ros::spinOnce();
currentState = nextState;
}
}
bool Explore::isCurrentGoalDone(){
bool is_done = ac_.getState().isDone();
return is_done;
}
bool goToGoal(const tf::TransformListener &listener, MoveBaseClient& ac, move_base_msgs::MoveBaseGoal& goal)
{
bool result = true;
double distanceToGoal = goalVectorLength(goal);
goal.target_pose.header.frame_id = BASE_LINK;
goal.target_pose.header.stamp = ros::Time::now();
ROS_INFO("Sending goal...");
ac.sendGoal(goal);
waitForTransformation(listener, BASE_LINK, ODOM_LINK);
//we will record transforms here
tf::StampedTransform start_transform;
tf::StampedTransform current_transform;
//record the starting transform from the odometry to the base frame
listener.lookupTransform(BASE_LINK, ODOM_LINK, ros::Time(0), start_transform);
ros::Rate rate(50.0);
ros::Duration waitDuration(5.0);
bool waitForGoal = true;
unsigned int goalReachTries = 0;
ROS_INFO("Waiting for goal to be reached...");
while (ros::ok() && waitForGoal && (goalReachTries < MAX_RETRY_COUNT))
{
ROS_INFO("Waiting for goal result...");
ac.waitForResult(waitDuration);
ros::spinOnce();
rate.sleep();
//get the current transform
try
{
listener.lookupTransform(BASE_LINK, ODOM_LINK, ros::Time(0), current_transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
break;
}
//see how far we've traveled
tf::Transform relative_transform = start_transform.inverse() * current_transform;
double distance = relative_transform.getOrigin().length();
if (distance >= (distanceToGoal - SMALL_ERROR))
{
waitForGoal = false;
ROS_INFO("Goal reached");
}
else
{
goalReachTries++;
ROS_INFO("Goal not reached. Attempt: %d distance: %f", goalReachTries, distance);
}
}
if (goalReachTries >= MAX_RETRY_COUNT)
{
result = false;
ROS_ERROR("Goal was not reached with given %d retries", MAX_RETRY_COUNT);
}
return result;
}
