Why my Straight line planner doesn't work on all maps?

edit

I'm working on an elementary but dramatic global planner. It should plan a simple straight-line path from a starting point, which is the baseline center, and a goal, which is a 2D nav goal that I set from the GUI of RVIZ, or by hands putting the coordinates of the goal. The job is simple, but I can't work it out. What I have done till now is, pick up the coordinates of the starting point and the goal (expressed on the map frame), calculate the angular coefficient, and using this coefficient calculate the x coordinate of each waypoint on the straight by applying a translation of delta (which is the increment I want to apply) and a rotation of angular coefficient. Then the y coordinate is calculated by simple linear interpolation. It turns out that the path is correct for some maps (generated using the famous Gmapping), but for others, it is not. It really depends on how the baseline frame is positioned relative to the map frame (sometimes the path is turned 180 degrees from where it should be), and I'm wondering if I'm encountering a problem with the representation of the Euler angles. I know it is a stupid question and I'm wrong on something, but currently, I'm really struggling.

float yaw = 0.f;

if(flag_){
  ROS_INFO("STRAIGHT");
  yaw = tf::getYaw(new_start.pose.orientation);
}else{
  ROS_INFO("non STRAIGHT");
  yaw = (goal_.pose.position.y - start.pose.position.y) / (goal_.pose.position.x - start.pose.position.x); //angular coefficient
}

int id = 0;
int n = 0;
float goal_distance = sqrt(pow((start.pose.position.x - goal_.pose.position.x), 2) + pow((start.pose.position.y - goal_.pose.position.y), 2));

while(n * delta < goal_distance){

  geometry_msgs::PoseStamped new_goal = goal_;        

  if (flag_)
  {
    //finding the points of the path that connect the goal (which is the projection of the start, n meters away) and the start, currently working on all maps
    new_goal.pose.position.x = (n * delta) * cos(yaw) + start.pose.position.x;
    new_goal.pose.position.y = (n * delta) * sin(yaw) + start.pose.position.y;
    new_goal.pose.orientation = new_start.pose.orientation;
  }

  else
  {        
        //finding the points of the path that connect the goal and the start, currently not working on some maps
        sign = (goal_.pose.position.x - start.pose.position.x >= 0) ? 1 : -1; //doesn't make sense, but it works
    new_goal.pose.position.x = sign * (n * delta) * cos(yaw) + start.pose.position.x;
    new_goal.pose.position.y = (((new_goal.pose.position.x - start.pose.position.x) * (goal_.pose.position.y - start.pose.position.y)) / (goal_.pose.position.x - start.pose.position.x)) + start.pose.position.y;
    new_goal.pose.orientation = goal_.pose.orientation;

  }
  ++n;
  plan.push_back(new_goal);
}

This planner draws a straight line using as a goal the projection of the current pose meters away, or a straight line using as a goal the point selected from RVIZ. As you can see, in the second part, there is a lot going on, that's 100% wrong. Firstly I ... (more)