#include <robot/obstacles.h>
#include <geometry_msgs/Point.h>

//param pose_x,pose_y other robot current pose
vector< vector<geometry_msgs::Point> > obstacles::getObstacleArray(int pose_x, int pose_y)
{
    vector<geometry_msgs::Point> obstaclePoint;
    geometry_msgs::Point point;

    //first point, left-top point
    point.x = pose_x-0.5104;
    point.y = pose_y+0.5104;
    point.z = 0;
    obstaclePoint.push_back(point);

    //second point, left-down point
    point.x = pose_x-0.5104;
    point.y = pose_y-0.5104;
    point.z = 0;
    obstaclePoint.push_back(point);

    //third point, right-down point
    point.x = pose_x+0.5104;
    point.y = pose_y-0.5104;
    point.z = 0;
    obstaclePoint.push_back(point);

    //fourth point, right-top point
    point.x = pose_x+0.5104;
    point.y = pose_y+0.5104;
    point.z = 0;
    obstaclePoint.push_back(point);

    //first point again to complete the box
    point.x = pose_x-0.5104;
    point.y = pose_y+0.5104;
    point.z = 0;
    obstaclePoint.push_back(point);

    obstacleArray.push_back(obstaclePoint);
    return obstacleArray;
}
