Robotics StackExchange | Archived questions

RRT planner path passing through wall

RVIZ image 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 = get_closest_cell(nxt_pt, head_cell->children[i]);

        double dis = heu(nxt_pt, curr_cell->point);

        if(dis < mn_dis) {

          mn_dis = dis;
          best_cell = curr_cell;

        }

      }

      return best_cell;

  }


  GlobalPlanner::Point GlobalPlanner::generate_next_goal(){



      std_msgs::ColorRGBA blue;
      blue.r = 0;
      blue.g = 0;
      blue.b = 1.0;
      blue.a = 1.0;
      std_msgs::ColorRGBA red;
      red.r = 1.0;
      red.g = 0;
      red.b = 0;
      red.a = 1.0;
      std_msgs::ColorRGBA green;
      green.r = 0;
      green.g = 1.0;
      green.b = 0;
      green.a = 1.0;

      Point nxt_point = {};

      bool point_found = true;



      while(true) {

        bool obs_found = 0 ;

        __uint32_t mx_ , my_, mx_i, my_i;
        double wx_, wy_;


        mx_= rand() % (map_xf - map_xi + 1) + (map_xi);
        my_= rand() % (map_yf - map_yi + 1) + (map_yi);

        if(mx_ >= map_xf || mx_ <= map_xi || my_ >= map_yf || my_ <= map_yi) {

          cout << "out of bound point found! ---- SKIPPING" << endl;
          continue;

        }

        costmap_ros_->mapToWorld(mx_, my_, wx_, wy_);

        visualization_msgs::Marker marker;
        marker.header.frame_id = "map";
        marker.header.stamp = ros::Time();
        marker.ns = nh_.getNamespace();

        marker.id = marker_id_cnt++;

        marker.type = visualization_msgs::Marker::CUBE;
        marker.action = visualization_msgs::Marker::ADD;

        marker.pose.position.x = wx_;
        marker.pose.position.y = wy_;
        marker.pose.position.z = 1;
        marker.pose.orientation.x = 0.0;
        marker.pose.orientation.y = 0.0;
        marker.pose.orientation.z = 0.0;
        marker.pose.orientation.w = 1.0;

        marker.lifetime = ros::Duration(5.0);

        marker.scale.x = 0.5;
        marker.scale.y = 0.5;
        marker.scale.z = 0.5;

        marker.color.a = 1.0; // Don't forget to set the alpha!

        marker.color = green;

        //unsigned char cell_cost = costmap_ros_->getCost(mx_, my_);

        bool flag = check_cell_neighbour(Point{mx_, my_});


        /*if(cell_cost == costmap_2d::LETHAL_OBSTACLE) { 

          obs_found = 1;  
          cout << "Point lies on an obstacle!" << endl;
          marker.color = red;
          marker.scale.x = 0.5; 
          marker.scale.y = 0.5;
          goal_marker_pub.publish(marker);
          continue;

        }*/

        if(!flag) {

          obs_found = 1;  
          cout << "Point lies on an obstacle!" << endl;
          marker.color = red;
          marker.scale.x = 0.5; 
          marker.scale.y = 0.5;
          goal_marker_pub.publish(marker);
          continue;


        }

        goal_marker_pub.publish( marker );        


        point_found = true;    
        nxt_point = Point{mx_, my_};
        return nxt_point;

      }

      cout << "Something is wrong --- generate_next_goal function!" << endl;
      cout << "Sleeping for 5 seconds!" << endl;
      ros::Duration(5.0).sleep();

      return Point{};       

  }



void GlobalPlanner::print_world_params(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, __uint32_t &mx_i, __uint32_t &my_i, __uint32_t &mx_f, __uint32_t &my_f){

    double wx_i, wy_i, wx_f, wy_f;

    wx_i = start.pose.position.x; 
    wy_i = start.pose.position.y;
    wx_f = goal.pose.position.x;
    wy_f = goal.pose.position.y;

    costmap_ros_->worldToMap(wx_i, wy_i, mx_i, my_i);
    costmap_ros_->worldToMap(wx_f, wy_f, mx_f, my_f);

    cout << "Printing start pose and goal pose: " << endl;
    cout << "w_i: (" << wx_i << "," << wy_i << ") m_i: (" << mx_i << "," << my_i <<")" << endl;;
    cout << "w_f: (" << wx_f << "," << wy_f << ") m_f: (" << mx_f << "," << my_f <<")" << endl;;
    cout << endl;

  }




double GlobalPlanner::heu(Point p1, Point p2) {

    if(p1.x < p2.x) {swap(p1.x, p2.x);}
    if(p1.y < p2.y) {swap(p1.y, p2.y);}

    __uint32_t sum = ((int)p1.x - (int)p2.x) * ((int)p1.x - (int)p2.x) + ((int)p1.y - (int)p2.y) * ((int)p1.y - (int)p2.y);

      return (sqrt(sum));

  }

header file -

/** include the libraries you need in your planner here */
/** for global path planner interface */
#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_core/base_global_planner.h>
#include <geometry_msgs/PoseStamped.h>
#include <angles/angles.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/costmap_model.h>
#include <map>
#include<nav_msgs/Path.h>


#ifndef GLOBAL_PLANNER_CPP
#define GLOBAL_PLANNER_CPP



namespace global_planner {

    class GlobalPlanner : public nav_core::BaseGlobalPlanner {


        struct Point {

                __uint32_t x, y; 

                bool operator==(const Point &p1 ) const{   return ((p1.x == x) && (p1.y == y));  }   

                bool operator<(const Point &p1 ) const{    return ((p1.x < x) || (p1.x == x && p1.y < y) ) ;  }   

            };


        struct Cell {

            Point point; 
            __uint32_t cost_till_now;

            bool operator<(const Cell &c1) const {


                return (c1.cost_till_now < cost_till_now || (c1.cost_till_now == cost_till_now && c1.point < point));

            }

        };

        struct RRT_Cell{

            Point point;
            RRT_Cell* parent;
            std::vector<RRT_Cell*> children;

        };



        public:

            GlobalPlanner();

            GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
            void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
            bool makePlan(const geometry_msgs::PoseStamped& start,const geometry_msgs::PoseStamped& goal,std::vector<geometry_msgs::PoseStamped>& plan);



        private: 


            double heu(Point p1, Point p2);
            void update_planner_plan(std::vector<Point> &path_points, std::vector<geometry_msgs::PoseStamped> &plan, const geometry_msgs::PoseStamped &goal); 
            void publish_global_path(const std::vector<geometry_msgs::PoseStamped> &plan, const geometry_msgs::PoseStamped &goal);
            bool print_cell(const Cell &cell);
            void print_world_params(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, __uint32_t &mx_i, __uint32_t &my_i, __uint32_t &mx_f, __uint32_t &my_f);
            Point generate_next_goal();
            void update_map_bounds();
            RRT_Cell* get_closest_cell(const Point &nxt_pt, RRT_Cell* head_cell);
            bool RRT_path_so_far(RRT_Cell *head);
            void publish_marker_point(const Point &pt);
            void update_RRT_path_points(std::vector<Point> &path_points, RRT_Cell* final_cell);
            void update_RRT_planner_plan(std::vector<geometry_msgs::PoseStamped> &plan, const geometry_msgs::PoseStamped &goal, const std::vector<Point> &path_points);
            bool check_cell_neighbour(const Point &pt);


            costmap_2d::Costmap2D* costmap_ros_;
            costmap_2d::Costmap2DROS *my_costmap_ros;
            __uint32_t size_x, size_y;
            __uint32_t map_xi, map_xf, map_yi, map_yf;
            ros::Publisher global_plan_pub, goal_marker_pub;
            ros::Subscriber pose_sub;
            ros::NodeHandle nh_;
            int marker_id_cnt;
    };

};

#endif

Asked by skpro19 on 2021-06-19 18:16:38 UTC

Comments

Answers