fix

config/costmap_common_params.yaml

@@ -1,5 +1,4 @@
-# position_planner_name: PNKXLocalPlanner
+position_planner_name: PNKXLocalPlanner
-position_planner_name: HybridLocalPlanner
 docking_planner_name: PNKXDockingLocalPlanner
 go_straight_planner_name: PNKXGoStraightLocalPlanner
 rotate_planner_name: PNKXRotateLocalPlanner
@@ -27,29 +26,29 @@ virtual_walls_map:
   lethal_cost_threshold: 100
 
 obstacles:
-  observation_sources:  b_scan_marking b_scan_clearing #f_scan_marking f_scan_clearing
+  observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing 
-  # f_scan_marking:
+  f_scan_marking:
-  #   topic: /f_scan
+    topic: /f_scan
-  #   data_type: LaserScan
+    data_type: LaserScan
-  #   clearing: false
+    clearing: false
-  #   marking: true
+    marking: true
-  #   inf_is_valid: false
+    inf_is_valid: true
-  #   min_obstacle_height: 0.0
+    min_obstacle_height: 0.0
-  #   max_obstacle_height: 0.25
+    max_obstacle_height: 0.25
-  # f_scan_clearing:
+  f_scan_clearing:
-  #   topic: /f_scan
+    topic: /f_scan
-  #   data_type: LaserScan
+    data_type: LaserScan
-  #   clearing: true
+    clearing: true
-  #   marking: false
+    marking: false
-  #   inf_is_valid: false
+    inf_is_valid: true
-  #   min_obstacle_height: 0.0
+    min_obstacle_height: 0.0
-  #   max_obstacle_height: 0.25
+    max_obstacle_height: 0.25
   b_scan_marking:
     topic: /b_scan
     data_type: LaserScan
     clearing: false
     marking: true
-    inf_is_valid: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
   b_scan_clearing:
@@ -57,7 +56,7 @@ obstacles:
     data_type: LaserScan
     clearing: true
     marking: false
-    inf_is_valid: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
 

config/hybrid_local_planner_params.yaml

@@ -1,54 +0,0 @@
-LocalPlannerAdapter:
-  library_path: liblocal_planner_adapter
-  yaw_goal_tolerance: 0.017  
-  xy_goal_tolerance: 0.03 
-  min_approach_linear_velocity: 0.06
-
-HybridLocalPlanner:
-# base_local_planner: "hybrid_local_planner/HybridLocalPlanner"
-# HybridLocalPlanner:
-    library_path: libhybrid_local_planner
-    odom_topic: odom
-    # Trajectory
-
-    max_global_plan_lookahead_dist: 4.0
-    global_plan_viapoint_sep: 0.5
-    global_plan_prune_distance: 0.0
-    global_plan_goal_sep: 0.05
-
-    # Robot
-
-    max_vel_x: 1.0
-    max_vel_x_backwards: -0.2
-    max_vel_theta: 0.5
-    acc_lim_x: 1.0
-    acc_lim_theta: 2.0
-    turn_around_priority: True
-    stop_dist: 0.5
-    dec_dist: 1.0
-
-
-    # GoalTolerance
-
-    xy_goal_tolerance: 0.05
-    yaw_goal_tolerance: 0.05
-
-    # Optimization
-
-    # PP Parameters
-    w_vel: 0.9
-    w_omega: 1.12
-    # DWA Parameters
-    robot_max_v: 0.4
-    robot_max_w: 0.4
-    enable_backward_motion: False
-    w_targetheading: 1.7
-    w_velocity: 0.2
-    w_clearance: 0.2
-    w_pathDistance: 0.05
-    time_horizon: 3.0
-    velocity_resolution: 0.015
-
-    segment_transition_threshold: 0.01 # Ngưỡng khoảng cách chuyển segment
-    calibration_factor: 1.5 # Hệ số hiệu chuẩn
-    use_obstacle_avoidance: true # Bật tắt tránh vật cản

src/Navigations/Packages/move_base/src/convert_data.cpp

@@ -1,7 +1,6 @@
 #include <move_base/convert_data.h>
 #include <robot/robot.h>
 #include <robot_costmap_2d/cost_values.h>
-#include <fstream>
 
 char* move_base::ConvertData::cost_translation_table_ = NULL;
 
@@ -42,19 +41,7 @@ move_base::ConvertData::~ConvertData()
 // prepare grid_ message for publication.
 void move_base::ConvertData::prepareGrid()
 {
-  printf("Preparing costmap data for publication\n");
+  // boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
-
-  std::ofstream file("/home/robotics/sonvh/pnkx_nav_core/obstacle/hybrid_local_planner/cfg/costmap_data.txt");
-
-  if (!file.is_open())
-  {
-      std::cout << "Cannot open file!" << std::endl;
-      return;
-  }
-
-  // boost::unique_lock<costmap_2d::Costmap2D::mutex_t> 
-  // lock(*(costmap_->getCostmap()->getMutex()));
-
   double resolution = costmap_->getCostmap()->getResolution();
 
   grid_.header.frame_id = global_frame_;
@@ -66,44 +53,22 @@ void move_base::ConvertData::prepareGrid()
 
   double wx, wy;
   costmap_->getCostmap()->mapToWorld(0, 0, wx, wy);
-
   grid_.info.origin.position.x = wx - resolution / 2;
   grid_.info.origin.position.y = wy - resolution / 2;
   grid_.info.origin.position.z = 0.0;
   grid_.info.origin.orientation.w = 1.0;
-
   saved_origin_x_ = costmap_->getCostmap()->getOriginX();
   saved_origin_y_ = costmap_->getCostmap()->getOriginY();
 
   grid_.data.resize(grid_.info.width * grid_.info.height);
 
-  file << "frame: " << global_frame_ << std::endl;
-  file << "time: " << robot::Time::now().toSec() << std::endl;
-  file << "resolution: " << resolution << std::endl;
-  file << "width: " << grid_.info.width << std::endl;
-  file << "height: " << grid_.info.height << std::endl;
-  file << "origin_x: " << grid_.info.origin.position.x << std::endl;
-  file << "origin_y: " << grid_.info.origin.position.y << std::endl;
-
   unsigned char* data = costmap_->getCostmap()->getCharMap();
-
+  for (unsigned int i = 0; i < grid_.data.size(); i++)
-  for (unsigned int y = 0; y < grid_.info.height; y++)
   {
-      for (unsigned int x = 0; x < grid_.info.width; x++)
+    grid_.data[i] = cost_translation_table_[ data[ i ]];
-      {
-          unsigned int i = y * grid_.info.width + x;
-
-          grid_.data[i] = cost_translation_table_[data[i]];
-
-          file << int(grid_.data[i]) << " ";
-      }
-      file << "\n";
   }
-
-  file.close();
 }
 
-
 void move_base::ConvertData::updateCostmap(robot_nav_msgs::OccupancyGrid &grid, robot_map_msgs::OccupancyGridUpdate &update, bool &is_updated)
 {
   boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getCostmap()->getMutex()));