added buffer to replanning behavior

config/navigation.lua

@@ -4,7 +4,7 @@ end
 
 NavigationParameters = {
   laser_topics = {
-    "/scan",
+    "/velodyne_2dscan",
     "/kinect_laserscan",
   };
   laser_frame = "base_link";
@@ -45,7 +45,11 @@ NavigationParameters = {
   local_costmap_radius = 10;
   global_costmap_inflation_size = 0.4;
   global_costmap_resolution = 0.2;
-  global_costmap_radius = 100;
+  global_costmap_radius = 70;
+  -- global_costmap_origin_x_positive = false;
+  global_costmap_origin_x = -50;
+  -- global_costmap_origin_y_positive = false;
+  global_costmap_origin_y = -50;
   range_min = 0.1;
   range_max = 10.0;
 };

src/navigation/navigation.cc

@@ -176,7 +176,7 @@ void Navigation::Initialize(const NavigationParameters& params,
   int local_costmap_size = 2*static_cast<int>(std::round(params_.local_costmap_radius/params_.local_costmap_resolution));
   costmap_ = costmap_2d::Costmap2D(local_costmap_size, local_costmap_size, params_.local_costmap_resolution, -params.local_costmap_radius, -params.local_costmap_radius);
   int global_costmap_size = 2*static_cast<int>(std::round(params_.global_costmap_radius/params_.global_costmap_resolution));
-  global_costmap_ = costmap_2d::Costmap2D(global_costmap_size, global_costmap_size, params_.global_costmap_resolution, -params.global_costmap_radius, -params.global_costmap_radius);
+  global_costmap_ = costmap_2d::Costmap2D(global_costmap_size, global_costmap_size, params_.global_costmap_resolution, params.global_costmap_origin_x, params.global_costmap_origin_y);
   planning_domain_ = GraphDomain(map_file, &params_);
 
   LoadVectorMap(map_file);
@@ -244,7 +244,6 @@ void Navigation::LoadVectorMap(const string& map_file){ //Assume map is given as
             if((sqrt(pow(j, 2) + pow(k, 2)) <= cell_inflation_size) && (mx + j >= 0) && (mx + j < x_max) 
             && (my + k >= 0) && (my + k < y_max)){
               inflation_cells.insert(global_costmap_.getIndex(mx + j, my + k));
-              // global_costmap_.setCost(mx + j, my + k, costmap_2d::LETHAL_OBSTACLE);
             }
           }
         }
@@ -601,6 +600,7 @@ vector<GraphDomain::State> Navigation::Plan(const Vector2f& initial,
 
       if(new_row >= 0 && new_row < static_cast<int>(costmap_.getSizeInCellsX()) && new_col >= 0 
       && new_col < static_cast<int>(costmap_.getSizeInCellsY()) && (costmap_.getCost(new_row, new_col) != costmap_2d::LETHAL_OBSTACLE)
+      && (costmap_.getCost(new_row, new_col) != costmap_2d::INSCRIBED_INFLATED_OBSTACLE) 
       && (cost.count(neighbor_index) == 0 || cost[current_index] + 1 < cost[neighbor_index])){
         double wx, wy;
         costmap_.mapToWorld(new_row, new_col, wx, wy);
@@ -708,20 +708,28 @@ bool Navigation::IntermediatePlanStillValid(){
 }
 
 bool Navigation::GetGlobalCarrot(Vector2f& carrot) {
-  return GetCarrot(carrot, true);
+  for(float carrot_dist = params_.carrot_dist; carrot_dist > params_.intermediate_carrot_dist; carrot_dist -= 0.5){
+    if(GetCarrot(carrot, true, carrot_dist)){
+      return true;
+    }
+  }
+  return false;
+  // return GetCarrot(carrot, true, params_.carrot_dist);
 }
 
 bool Navigation::GetIntermediateCarrot(Vector2f& carrot) {
-  return GetCarrot(carrot, false);
+  return GetCarrot(carrot, false, params_.intermediate_carrot_dist);
 }
 
-bool Navigation::GetCarrot(Vector2f& carrot, bool global) {
-  float carrot_dist = params_.intermediate_carrot_dist;
+bool Navigation::GetCarrot(Vector2f& carrot, bool global, float carrot_dist) {
+  // float carrot_dist = params_.intermediate_carrot_dist;
+  // float carrot_dist = carrot_dist_test;
+
   vector<GraphDomain::State> plan_path = plan_path_;
-  if(global){
-    plan_path = global_plan_path_;
-    carrot_dist = params_.carrot_dist;
-  }
+  // if(global){
+  //   plan_path = global_plan_path_;
+  //   carrot_dist = params_.carrot_dist;
+  // }
   // const float kSqCarrotDist = Sq(params_.carrot_dist);
   const float kSqCarrotDist = Sq(carrot_dist);
 
@@ -1093,6 +1101,7 @@ bool Navigation::Run(const double& time,
                      Vector2f& cmd_vel,
                      float& cmd_angle_vel) {
   const bool kDebug = FLAGS_v > 0;
+  cout << "begin run" << endl;
   if (!initialized_) {
     if (kDebug) printf("Parameters and maps not initialized\n");
     return false;
@@ -1129,12 +1138,14 @@ bool Navigation::Run(const double& time,
   costmap_.resetMap(0, 0, x_max, y_max);
   costmap_obstacles_.clear();
 
-  unordered_set<uint64_t> inflation_cells;
-  unordered_set<uint64_t> obstacle_cells;
+  // True if distance is less than replan inflation size
+  // Assign different value for points within inflation size but farther than replan size
+  unordered_map<uint32_t, bool> inflation_cells;
+  unordered_set<uint32_t> obstacle_cells;
 
   // Map from costmap index to real coordinates relative to robot
-  unordered_map<uint64_t, float> index_to_x_coord;
-  unordered_map<uint64_t, float> index_to_y_coord;
+  unordered_map<uint32_t, float> index_to_x_coord;
+  unordered_map<uint32_t, float> index_to_y_coord;
 
   uint32_t robot_mx, robot_my;
   costmap_.worldToMap(0, 0, robot_mx, robot_my);
@@ -1145,8 +1156,13 @@ bool Navigation::Run(const double& time,
   costmap_.mapToWorld(robot_row, robot_col, robot_wx, robot_wy);
   Vector2f robot_location(robot_wx, robot_wy);
 
+  cout << "add points to costmap" << endl;
+
   // Add new points to costmap
   for (size_t i = 0; i < point_cloud_.size(); i++){
+    if(point_cloud_[i].x() < -0.5f && point_cloud_[i].y() > 0){
+      cout << "point: (" << point_cloud_[i].x() << ", " << point_cloud_[i].y() << ")" << endl;
+    }
     uint32_t unsigned_mx, unsigned_my;
     Vector2f relative_location_map_frame = Rotation2Df(robot_angle_) * point_cloud_[i];
     bool in_map = costmap_.worldToMap(relative_location_map_frame.x(), relative_location_map_frame.y(), unsigned_mx, unsigned_my); 
@@ -1216,7 +1232,7 @@ bool Navigation::Run(const double& time,
     iter++;
   }
 
-
+  // TODO: test
   // Add old obstacles that are still in map to new costmap
   for (const auto& point : prev_obstacles_) {
     uint32_t unsigned_mx, unsigned_my;
@@ -1240,24 +1256,43 @@ bool Navigation::Run(const double& time,
       for (int k = -cell_inflation_size; k <= cell_inflation_size; k++){
         if((sqrt(pow(j, 2) + pow(k, 2)) <= cell_inflation_size) && (mx + j >= 0) && (mx + j < x_max) 
         && (my + k >= 0) && (my + k < y_max)){
-          inflation_cells.insert(costmap_.getIndex(mx + j, my + k));
+          if((sqrt(pow(j, 2) + pow(k, 2)) <= params_.replan_inflation_size))
+            inflation_cells[costmap_.getIndex(mx + j, my + k)] = true;
+          else
+            inflation_cells[costmap_.getIndex(mx + j, my + k)] = false;
+          // inflation_cells.insert(costmap_.getIndex(mx + j, my + k));
         }
       }
     }
   }
 
-
-  for (const auto& index : inflation_cells) {
+  for (const auto& pair : inflation_cells) {
+    uint32_t index = pair.first;
+    bool obstacle_inflation = pair.second;
     uint32_t mx = 0;
     uint32_t my = 0;
     costmap_.indexToCells(index, mx, my);
-    costmap_.setCost(mx, my, costmap_2d::LETHAL_OBSTACLE);
+    if(obstacle_inflation)
+      costmap_.setCost(mx, my, costmap_2d::LETHAL_OBSTACLE);
+    else
+      costmap_.setCost(mx, my, costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
 
     double wx, wy;
     costmap_.mapToWorld(mx, my, wx, wy);
     costmap_obstacles_.push_back(Vector2f(wx, wy) + robot_loc_);
   }
 
+  // for (const auto& index : inflation_cells) {
+  //   uint32_t mx = 0;
+  //   uint32_t my = 0;
+  //   costmap_.indexToCells(index, mx, my);
+  //   costmap_.setCost(mx, my, costmap_2d::LETHAL_OBSTACLE);
+
+  //   double wx, wy;
+  //   costmap_.mapToWorld(mx, my, wx, wy);
+  //   costmap_obstacles_.push_back(Vector2f(wx, wy) + robot_loc_);
+  // }
+
   // Record last seen locations of points
   prev_robot_loc_ = robot_loc_;
   prev_obstacles_.clear();

src/navigation/navigation.h

@@ -126,7 +126,7 @@ class Navigation {
 
   bool GetGlobalCarrot(Eigen::Vector2f& carrot);
   bool GetIntermediateCarrot(Eigen::Vector2f& carrot);
-  bool GetCarrot(Eigen::Vector2f& carrot, bool global);
+  bool GetCarrot(Eigen::Vector2f& carrot, bool global, float carrot_dist);
   // Enable or disable autonomy.
   void Enable(bool enable);
   // Indicates whether autonomy is enabled or not.

src/navigation/navigation_main.cc

@@ -487,7 +487,7 @@ void DrawTarget() {
   const Eigen::Vector2f target = navigation_.GetTarget();
   auto msg_copy = global_viz_msg_;
   visualization::DrawCross(navigation_.GetIntermediateGoal(), 0.2, 0x0000FF, global_viz_msg_);
-  visualization::DrawCross(target, 0.2, 0x10E000, msg_copy);
+  // visualization::DrawCross(target, 0.2, 0x10E000, msg_copy);
   visualization::DrawArc(
       Vector2f(0, 0), carrot_dist, -M_PI, M_PI, 0xE0E0E0, local_viz_msg_);
   viz_pub_.publish(msg_copy);
@@ -787,12 +787,15 @@ void LoadConfig(navigation::NavigationParameters* params) {
   STRING_PARAM(model_path);
   STRING_PARAM(evaluator_type);
   STRING_PARAM(camera_calibration_path);
-  REAL_PARAM(local_costmap_resolution)
-  REAL_PARAM(local_costmap_inflation_size)
-  REAL_PARAM(local_costmap_radius)
-  REAL_PARAM(global_costmap_resolution)
-  REAL_PARAM(global_costmap_inflation_size)
-  REAL_PARAM(global_costmap_radius)
+  REAL_PARAM(local_costmap_resolution);
+  REAL_PARAM(local_costmap_inflation_size);
+  REAL_PARAM(local_costmap_radius);
+  REAL_PARAM(replan_inflation_size);
+  REAL_PARAM(global_costmap_resolution);
+  REAL_PARAM(global_costmap_inflation_size);
+  REAL_PARAM(global_costmap_radius);
+  REAL_PARAM(global_costmap_origin_x);
+  REAL_PARAM(global_costmap_origin_y);
   REAL_PARAM(intermediate_carrot_dist);
   REAL_PARAM(range_min);
   REAL_PARAM(range_max);
@@ -828,9 +831,12 @@ void LoadConfig(navigation::NavigationParameters* params) {
   params->local_costmap_resolution = CONFIG_local_costmap_resolution;
   params->local_costmap_inflation_size = CONFIG_local_costmap_inflation_size;
   params->local_costmap_radius = CONFIG_local_costmap_radius;
+  params->replan_inflation_size = CONFIG_replan_inflation_size;
   params->global_costmap_resolution = CONFIG_global_costmap_resolution;
   params->global_costmap_inflation_size = CONFIG_global_costmap_inflation_size;
   params->global_costmap_radius = CONFIG_global_costmap_radius;
+  params->global_costmap_origin_x = CONFIG_global_costmap_origin_x;
+  params->global_costmap_origin_y = CONFIG_global_costmap_origin_y;
   params->intermediate_carrot_dist = CONFIG_intermediate_carrot_dist;
   params->range_min = CONFIG_range_min;
   params->range_max = CONFIG_range_max;
@@ -967,14 +973,12 @@ int main(int argc, char** argv) {
       auto obstacles = navigation_.GetCostmapObstacles();
       auto global_obstacles = navigation_.GetGlobalCostmapObstacles();
 
-      for (const auto& vector : global_obstacles) {
-        // if(vector.y() < 2 && vector.x() < 6)
-          visualization::DrawPoint(vector, 0xdb34eb, global_viz_msg_);
-      }
+      // for (const auto& vector : global_obstacles) {
+      //   visualization::DrawPoint(vector, 0xdb34eb, global_viz_msg_);
+      // }
 
       for (const auto& vector : obstacles) {
-        // if(vector.y() < 2 && vector.x() < 6)
-          visualization::DrawPoint(vector, 0x10E000, global_viz_msg_);
+        visualization::DrawPoint(vector, 0x10E000, global_viz_msg_);
       }
 
       PublishForwardPredictedPCL(navigation_.GetPredictedCloud());

src/navigation/navigation_parameters.h

@@ -105,11 +105,15 @@ struct NavigationParameters {
   float local_costmap_resolution;
   // Costmap size in each direction
   float local_costmap_radius;
+  // Minimum inflation distance to change intermediate plan
+  float replan_inflation_size;
 
   // Same as local costmap parameters but for global costmap
   float global_costmap_inflation_size;
   float global_costmap_resolution;
   float global_costmap_radius;
+  float global_costmap_origin_x;
+  float global_costmap_origin_y;
 
   // Lidar scan min range
   float range_min;
@@ -146,9 +150,12 @@ struct NavigationParameters {
       local_costmap_inflation_size(0.5),
       local_costmap_resolution(0.1),
       local_costmap_radius(10),
+      replan_inflation_size(0.3),
       global_costmap_inflation_size(0.5),
       global_costmap_resolution(0.1),
       global_costmap_radius(100),
+      global_costmap_origin_x(-100),
+      global_costmap_origin_y(-100),
       range_min(0.1),
       range_max(10){
       }