RRT planner path passing through wall

edit

I am trying to write a RRT-based globalplanner in ROS. I am following the following strategy -

Generating a random point in the map -> selecting the best possible node from the current path -> moving a small distance in the direction of the random point.

Even though my planner is able to generate a plan from the start position to the goal position, it sometimes passes through obstacles even though before adding any cell to the path, I am checking the cell as well as the 5 *5 square around it for costmap_2d::LETHAL_OBSTACLE value.

I am attaching the relevant functions that I have used in my code-

bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,  std::vector<geometry_msgs::PoseStamped>& plan ){

    bool reached = false;

    __uint32_t mx_i, my_i, mx_f, my_f, mx_c, my_c;

    print_world_params(start, goal, mx_i, my_i, mx_f, my_f);

    mx_c = mx_i, my_c = my_i;

    RRT_Cell* head_cell = new RRT_Cell();
    head_cell->point = Point{mx_i, my_i};
    head_cell->parent = nullptr;

    vector<Point> path_points;

    while(true) {

      Point nxt_pt = generate_next_goal();

      RRT_Cell* best_cell = get_closest_cell(nxt_pt, head_cell);
      Point best_pt = best_cell->point;

      double dis_from_goal = heu(nxt_pt, Point{mx_f, my_f});

      if(dis_from_goal < 20) {

        cout << "Almost reached the goal!" <<endl;

        RRT_Cell* final_cell = new RRT_Cell();
        final_cell->point = Point{mx_f, my_f};
        final_cell->parent = best_cell;  
        best_cell->children.push_back(final_cell);

        update_RRT_path_points(path_points, final_cell);
        reverse(path_points.begin(), path_points.end());
        update_RRT_planner_plan(plan, goal, path_points);
        publish_global_path(plan, goal);


        reached = true;
        break;

      }


      __uint32_t step_sz = 20;

      bool valid_pt= true;

      double dis = heu(nxt_pt, Point{best_pt.x, best_pt.y});

      if(dis < 1){continue;}

      //dis = 0;
      double sin_th = double((int)nxt_pt.y - (int)best_pt.y) / dis;
      double cos_th = double((int)nxt_pt.x - (int)best_pt.x) / dis;
      cout << "sin_th: " << sin_th << " cos_th: " << cos_th << endl;
      //cout << "Sleeping for 1 second!" << endl;
      //ros::Duration(1.0).sleep();

      for(int i = 0; i <= step_sz; i++) {

        __uint32_t mx_d = (int)best_pt.x + i * cos_th;
        __uint32_t my_d = (int)best_pt.y+ i* sin_th;

        if(mx_d <= map_xi || mx_d >= map_xf || my_d <= map_yi || my_d >= map_yf) {

          valid_pt = false;
          break;

        }

        bool flag = check_cell_neighbour(Point{mx_d, my_d});

        if(!flag){

          valid_pt = false;
          break;

        }

      }


      if(!valid_pt) {continue;}

      mx_c = best_pt.x + step_sz * cos_th;
      my_c = best_pt.y + step_sz * sin_th;

      RRT_Cell* latest_cell = new RRT_Cell();
      latest_cell->point = Point{mx_c, my_c};
      latest_cell->parent = best_cell;

      best_cell->children.push_back(latest_cell);

    }


    cout <<"reached: " << reached << endl;


    if(!reached) {

      cout << "Something is wrong! ---- Could not reach near the goal!" << endl;

    }




    return true;

  }



  bool GlobalPlanner::check_cell_neighbour(const Point &pt) {


    int margin_sz = 5;

    for(int i = (int)pt.x - margin_sz; i < (int)pt.x + margin_sz; i++) {

      for(int j = (int)pt.y - margin_sz; j <= (int)pt.y + margin_sz; j++) {

        if(i <= map_xi || j <= map_xi || i >= map_xf || j>= map_yf) {return false;}

        unsigned char cell_cost = costmap_ros_->getCost(i,j);

        if(cell_cost == costmap_2d::LETHAL_OBSTACLE) {return false;}

        //int cell_cost = costmap_ros_->getCost(i,j);

        //if(cell_cost > 200) {return false;}



      }

    }

    return true;


  }


 GlobalPlanner::RRT_Cell* GlobalPlanner::get_closest_cell(const Point &nxt_pt, RRT_Cell* head_cell) {

      if(head_cell == nullptr) {return nullptr;} 

      if(head_cell->children.size() == 0) {

        return head_cell;

      }

      long double mn_dis = heu(nxt_pt, head_cell->point); 
      RRT_Cell* best_cell = head_cell;

      for(int i = 0; i < (int)head_cell->children.size(); i++) {

        RRT_Cell* curr_cell ...