/*
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);
}
}
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());
}
}
}
}