/*
Returns true if known toys in same side as robot or in middle toy area
Otherwise returns false
*/
bool isToyToPick() {
	if (knownToys.size() < 1) {
		return false;
	}
	else {
		geometry_msgs::Point robotPos = getRobotPosition();
		int robotArea = inArea(robotPos.x,robotPos.y);
        if (robotArea==MIDDLE_ROBOT_AREA || robotArea==MIDDLE_TOY_AREA){
            return false; //If robot in middle area, do not start toy pick up procedures
		}
		else {
            int number_toys_to_pick = 0;
			for (float2DVector::size_type i = 0; i < knownToys.size(); i++) { 
				int toyArea = inArea(knownToys[i][0],knownToys[i][1]);
                
				if (robotArea == toyArea || toyArea == MIDDLE_TOY_AREA){
                    number_toys_to_pick++;
				}
			}
			ROS_INFO("%d toys to pick from this side",number_toys_to_pick);
			if (number_toys_to_pick>0){
			  return true;
			}
			else{
			  return false;
			}
		} 
	}
}
/*
Adds new toy coordinates to knownToys vector
Updates the coordinates if toy already known
Input:  float x == x
        float y == y
        float z == toy_type (wooden == 1, small = 0)
*/
void update_toys(float x, float y, float z)
{
    bool found = false;
    float dist = 9999;
    //Check if the new toy coordinate is inside the radius of any of the known toys

    for (float2DVector::size_type i = 0; i < knownToys.size(); i++) {
        dist = sqrt(pow(x - knownToys[i][0],2) + pow(y - knownToys[i][1],2));
        if (dist < SAME_TOY_MAX_R) {
            //inside toy radius, update the toy coordinate
            knownToys[i][0] = (knownToys[i][0] + x) / 2;
            knownToys[i][1] = (knownToys[i][1] + y) / 2;
            found = true;
            break;
        }
    }
    if (!found) {
        ROS_INFO("A new toy!");
        vector<float> pos;
        pos.push_back(x);
        pos.push_back(y);
        pos.push_back(z);
        knownToys.push_back(pos);
    }
}
Beispiel #3
0
float HolisticRecognizer::holisticDistance(const float2DVector& test,const HolisticShapeModel& train, float bestSoFar)
{
	LOG( LTKLogger::LTK_LOGLEVEL_DEBUG) << "Entered HolisticRecognizer::holisticDistance"  << endl;

	float distance=0.0f;		// distance between testVec and floatVec

	int loopIndex;				// Index used for looping
	
	// check and validate the test vector size

	if (test.size()!=1)
	{
		throw LTKException(ECOMPUTE_DISTANCE_ERROR);
	}

	floatVector testVec = test.at(0);

	// check and validate the train vector size
	
	if ((train.getModel()).size()!=1)
	{
		throw LTKException(ECOMPARISON_ERROR);
	}
	
	floatVector trainVec = (train.getModel()).at(0);

	int vecSize = testVec.size();

	// check and validate the train & test vector sizes

	if (vecSize!=trainVec.size())
	{
		throw LTKException(ETRAIN_TEST_VECTOR_SIZE_MISMATCH);
	}

	// calculating the euclidean distance 

	for(loopIndex = 0; loopIndex < vecSize; ++loopIndex)
	{
		distance += (testVec.at(loopIndex)-trainVec.at(loopIndex))*(testVec.at(loopIndex)-trainVec.at(loopIndex));
	}

	distance = sqrt(distance);

	LOG( LTKLogger::LTK_LOGLEVEL_DEBUG) << "Exiting HolisticRecognizer::holisticDistance"  << endl;

	return distance;
}
/*
Main loop: Initialise and Higher level planning
*/
int main(int argc, char** argv)
{
    //Ros inits
    ros::init(argc, argv, "final_competition");
    ros::NodeHandle n;
    cmd_vel_pub = n.advertise<geometry_msgs::Twist>("RosAria/cmd_vel", 10);
    gripper_pub = n.advertise<geometry_msgs::Vector3>("gripper", 10);
    toy_sub = n.subscribe("toy_coordinates", 10, checkInput);
    pose_sub = n.subscribe("/amcl_pose", 10, updatePosition);
    MoveBaseClient ac("move_base", true);
    
    goalCmd.target_pose.pose.position.x = 999;
    goalCmd.target_pose.pose.position.y = 999;
    
    //Values for blind movement
    go_back.linear.x    = -0.4;
    go_back.angular.x   = 0.0;
    go_back.angular.y   = 0.0;
    go_back.angular.z   = 0.0;
    go_forward.linear.x  = 0.4;
    go_forward.angular.x = 0.0;
    go_forward.angular.y = 0.0;
    go_forward.angular.z = 0.0;
    stop.linear.x       = 0.0;
    stop.angular.x      = 0.0;
    stop.angular.y      = 0.0;
    stop.angular.z      = 0.0;
    
    //Control values for gripper
    up_open.x = 2;
    up_open.y = 0;
    up_close.x = 2;
    up_close.y = 1;
    middle_gripper_open.x = 1;
    middle_gripper_open.y = 0;
    middle_gripper_close.x = 1;
    middle_gripper_close.y = 1;
    down_open.x = 0;
    down_open.y = 0;
    down_close.x = 0;
    down_close.y = 1;
	
    ROS_INFO("\n\n\n\n\n\n");

    //wait for the action server to come up
    while(ros::ok() && !ac.waitForServer(ros::Duration(5.0))){
        ROS_INFO("Waiting for the move_base action server to come up");
    }
    ROS_INFO("move_base action server is up");
    sendNewGoal = true;

    //gripper down and closed for movement
    gripperDownAndClosed();

    //Initial spin to scan the area
    spinRobot(ac);

    ros::Rate rate(ROS_RATE_FREQ);
    while(ros::ok()) {

        //Check if known toys on the same side of robot
        //If no toys, do search procedure
        //If toys, get toy
        if (!isToyToPick()) {

            //If known toys in array, but none on robot side, next search position on other side
            if (knownToys.size() > 0) {
              geometry_msgs::Point robotPos = getRobotPosition();
              int robotArea = inArea(robotPos.x,robotPos.y); //get robot side
              nextSearchPlace = robotArea+1; //changes to other side
            }
            ROS_INFO("No toys going to search place %d",(nextSearchPlace+1));
           
            //Goto next search place
            robotGoToNextSearchPlace(ac);

            //If still no toys on same side, search spin
            if (!isToyToPick()) {
              spinRobot(ac);
              nextSearchPlace++;
            }
            ROS_INFO("Robot spinned");
        
        }

        else {
            ROS_INFO("Toys found in array, getting closest toy");

            //Get coordinates for next toy to pick up
            vector<float> nextPickUpPoint = getNextPickUpPoint();

            //Check if correct point form received
            if (nextPickUpPoint.size() > 3) {
                geometry_msgs::Point coord;
                ROS_INFO("Got toy coordinates, starting to use them");
                coord.x = nextPickUpPoint[0];
                coord.y = nextPickUpPoint[1];
                coord.z = 0;
                float toyIndex = nextPickUpPoint[3];
                ROS_INFO("Next pickup point: x = %f, y = %f, angle = %f, index = %f", nextPickUpPoint[0], nextPickUpPoint[1], nextPickUpPoint[2], nextPickUpPoint[3]);

                ROS_INFO("Approaching toy");
                bool approach_success = robotGoToCoord(coord, ac, nextPickUpPoint[2]);

                if (approach_success) {

                    ROS_INFO("Opening gripper for pick up");
                    gripperDownAndOpen();

                    ROS_INFO("Moving blindly to toy");
                    moveToObject(STEPS_DUMMY_FORWARD_TOY);

                    ROS_INFO("Picking up toy");
                    gripperDownAndClosed();

                    ROS_INFO("Stepping back from toy point");
                    moveAwayFromObject(STEPS_DUMMY_BACK_TOY);

                    ROS_INFO("Checking toy type");
                    geometry_msgs::Point basket_position;
                    float basket_angle;

                    if (isToyWooden(toyIndex)){
                        ROS_INFO("It was wooden!");
                        basket_position.x = WOOD_BASKET[0];
                        basket_position.y = WOOD_BASKET[1];
                        basket_angle = WOOD_BASKET[2];
                        }
                    else{
                        ROS_INFO("It was not wooden!");
                        basket_position.x = OTHER_BASKET[0];
                        basket_position.y = OTHER_BASKET[1];
                        basket_angle = OTHER_BASKET[2];
                    }

                    ROS_INFO("Going to basket");
                    bool basket_success = robotGoToCoord(basket_position, ac, basket_angle); // position is hard coded before basket
                    if (basket_success) {
                        ROS_INFO("Basket reached");
                        ROS_INFO("Lifting toy");
                        liftToyUp();

                        ROS_INFO("Moving blindly towards basket");
                        moveToObject(STEPS_DUMMY_FORWARD_BASKET);

                        ROS_INFO("Dropping toy");
                        dropToy();

                        ROS_INFO("Stepping back from basket");
                        moveAwayFromObject(STEPS_DUMMY_BACK);

                        ROS_INFO("Putting gripper down and closed");
                        gripperDownAndClosed();

                        ROS_INFO("Removing delivered toy from array");
                        removeToy(toyIndex);
                    }
                    else {
                        ROS_INFO("Failed to reach basket");
                        ROS_INFO("Dropping toy");
                        dropToy();

                        ROS_INFO("Stepping back from dropped toy");
                        moveAwayFromObject(STEPS_DUMMY_BACK_TOY);

                        ROS_INFO("Closing gripper for movement");
                        gripperDownAndClosed();

                        ROS_INFO("Removing unsuccesfully delivered toy from array");
                        removeToy(toyIndex);
                    }
                }
                else{
                    ROS_INFO("Failed to reach toy, remove toy from array");
                    removeToy(toyIndex);
                }
            }
		}
		ros::spinOnce();
        ROS_INFO("I'm tired, let's sleep\n\n\n\n\n\n");
		rate.sleep();
	}
	
    
    return 0;
}
/*
Returns vector<float> toyCoords, approach coordinates for the closest toy found on the same side as robot
Output: toyCoords[0] = x
        toyCoords[1] = y
        toyCoords[2] = angle to face toy
        toyCoords[3] = toy index in knownToys, for removing toy
*/
vector<float> getNextPickUpPoint() {
	vector<float> toyCoords;
    ROS_INFO("Inside getNextPickUpPoint");
	if (!isToyToPick()) { // there needs to be a toy to pick up
        ROS_INFO("Inside getNextPickUpPoint !isToyToPick()");
        toyCoords.push_back(0.0);
        return toyCoords; //Checked as fail at call
	}
	else {
        geometry_msgs::Point robotPos = getRobotPosition();
        int robotArea = inArea(robotPos.x,robotPos.y);

        int toyArea;
		vector<float> closestToy;
		float closestToyIndex;
		int closestToyArea;
        float closestDist = 9999;

        float middleAreaCost = 10; //meters

        //Find toy with the closest distance on same side as robot or middle toy area
		for (float2DVector::size_type i = 0; i < knownToys.size(); i++) { 
            toyArea = inArea(knownToys[i][0],knownToys[i][1]);
            if (robotArea == toyArea || toyArea == MIDDLE_TOY_AREA){ //toys from same or middle toy area
				float newClosestDist = sqrt(pow(knownToys[i][0] - robotPos.x, 2) + pow(knownToys[i][1] - robotPos.y, 2));
				if(MIDDLE_TOY_AREA == toyArea){
                    //add distance to middle area toys to pick up same side toys first
					newClosestDist = newClosestDist + middleAreaCost;            
				}
                if (newClosestDist < closestDist) {
                    //closer toy found, update to closest
					closestDist = newClosestDist; 
					closestToy = knownToys[i]; 
					closestToyIndex = float(i); //index for removing toy
					closestToyArea = toyArea;
                    ROS_INFO("Inside getNextPickUpPoint NEWCLOSESTDIST");
				}
			}
		}
		float angle;

        //Calculate approach point and angle
        //If toy in middle toy area, approach direction is x or -x.
        if (closestToyArea == MIDDLE_TOY_AREA){
			if (robotArea == LEFT_AREA){
				toyCoords.push_back(closestToy[0] - STOP_DISTANCE_FROM_TOY* cos(UP));
				toyCoords.push_back(closestToy[1] - STOP_DISTANCE_FROM_TOY* sin(UP));
                angle = UP;
			}
			else { // robotArea == RIGHT_AREA	
				toyCoords.push_back(closestToy[0] + STOP_DISTANCE_FROM_TOY* cos(UP));
				toyCoords.push_back(closestToy[1] + STOP_DISTANCE_FROM_TOY* sin(UP));
				angle = UP + PI;
			}
		}
        //Else approach direction is angle from robot to toy
        else { // toyArea == LEFT_AREA || RIGHT_AREA
            robotPos = getRobotPosition();
            ROS_INFO("Inside getNextPickUpPoint STRAIGHT");
			angle = atan2((closestToy[1] - robotPos.y),(closestToy[0] - robotPos.x));
			toyCoords.push_back(closestToy[0] - STOP_DISTANCE_FROM_TOY* cos(angle));
			toyCoords.push_back(closestToy[1] - STOP_DISTANCE_FROM_TOY* sin(angle));
		}
		toyCoords.push_back(angle);
		toyCoords.push_back(closestToyIndex);
		ROS_INFO("Closest toy x coordinate is %f and y coordinate is %f",toyCoords[0],toyCoords[1]);
		return toyCoords;
	}
}
/*
Filters input coordinates
Called by subscriber to toy coordinates published by image processing
Calls update_toys when trust treshold for coordinate is reached
Obstacles are filtered out
Input:  input_coord->x: float x
        input_coord->y: float y
        input_coord->z: float toy_type (wooden == 1, small = 0)
*/
void checkInput(const geometry_msgs::Point::ConstPtr& input_coord) {	
    bool found = false;
    float dist = 9999; //initialise distance between new and old toy coordinates

    float x = input_coord->x;
    float y = input_coord->y;
	
    //Map area points for ignoring toy coordinates in obstacles:

    //Outside
    float map_x_begin   = -1.00;
	float map_x_end     = 2.63;
    float map_y_begin   = -1.62;
	float map_y_end     = 1.35;
    
    //Box
    float box_x_begin   = 0.44;
	float box_x_end     = 1.10;
	float box_y_begin   = 0.24;
	float box_y_end     = 0.80;
    
    //Pillar, currently not needed
    float pillar_x_begin   = 999.0;
	float pillar_x_end     = 999.0;
	float pillar_y_begin   = 999.0; 
	float pillar_y_end     = 999.0;
    
    //Middle wall
    float wall_x_begin   = 0.768;
	float wall_x_end     = 1.215; 
	float wall_y_begin   = -1.814;
	float wall_y_end     = -0.307;
    
    //White basket
    float white_x_begin   = -0.379;
	float white_x_end     = 0.004; 
	float white_y_begin   = -1.772;
	float white_y_end     = -1.539;
    
    //Blue basket, currently not needed
    float blue_x_begin   = 999.0;
	float blue_x_end     = 999.0;
	float blue_y_begin   = 999.0;
	float blue_y_end     = 999.0;
    
    // NOT (outside the map OR in box OR in pillar OR in middle wall OR in white box OR in blue box)
    if (!(x < map_x_begin || x > map_x_end || y < map_y_begin || y > map_y_end
            || (x > box_x_begin    && x < box_x_end    && y > box_y_begin    && y < box_y_end)
            || (x > pillar_x_begin && x < pillar_x_end && y > pillar_y_begin && y < pillar_y_end)
            || (x > wall_x_begin   && x < wall_x_end   && y > wall_y_begin   && y < wall_y_end)
            || (x > white_x_begin  && x < white_x_end  && y > white_y_begin  && y < white_y_end)
            || (x > blue_x_begin   && x < blue_x_end   && y > blue_y_begin   && y < blue_y_end) )) 
    {
        //Check if the new coordinate is inside the radius of any of the filtering table coordinates
        for (float2DVector::size_type i = 0; i < filteringTable.size(); i++) {
            dist = sqrt(pow(x - filteringTable[i][0],2) + pow(y - filteringTable[i][1],2));
            if (dist < SAME_TOY_MAX_R) {
                //Inside coordinate radius, update the filtering table coordinate
                filteringTable[i][0] = (filteringTable[i][0] + x) / 2;
                filteringTable[i][1] = (filteringTable[i][1] + y) / 2;
                filteringTable[i][3] = filteringTable[i][3] + 1;
                if (filteringTable[i][3] >= INPUT_TRUST_TRESHOLD) {
                    //coordinate has reached it input trust treshold, update toys
                    update_toys(filteringTable[i][0], filteringTable[i][1],filteringTable[i][2]);
                }
                found = true;
                break;
            }
        }
        if (!found) {
            //no matching toy, add a new toy to filtering table
            vector<float> pos;
            pos.push_back(x);
            pos.push_back(y);
            pos.push_back(input_coord->z);
            pos.push_back(1);
            filteringTable.push_back(pos);

            //If too much coordinates in filtering table, clear first ones
            while (filteringTable.size() > 50) {
                filteringTable.erase(filteringTable.begin());
            }
        }
    }
    
}