update

.gitignore

@@ -421,3 +421,6 @@ FodyWeavers.xsd
 build
 install
 devel
+obstacle
+/pnkx_nav_core/config/hybrid_local_planner_params.yaml
+

CMakeLists.txt

@@ -134,6 +134,22 @@ if (NOT TARGET pnkx_local_planner)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner)
 endif()
 
+if (NOT TARGET robot_angles)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/angles)
+endif()
+
+if (NOT TARGET grid_map_core)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/grid_map_core)
+endif()
+
+if (NOT TARGET robot_base_local_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/base_local_planner)
+endif()
+
+if (NOT TARGET hybrid_local_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/hybrid_local_planner)
+endif()
+
 if (NOT TARGET robot_actionlib_msgs)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_actionlib_msgs)
 endif()

config/config/obstacle_layer_params.yaml

@@ -1,4 +1,5 @@
 obstacle_layer:
+  enabled: true
   track_unknown_space: true
   transform_tolerance: 0.2
   topic: "map"

config/costmap_common_params.yaml

@@ -1,4 +1,5 @@
-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
@@ -26,23 +27,23 @@ virtual_walls_map:
   lethal_cost_threshold: 100
 
 obstacles:
-  observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
+  observation_sources:  b_scan_marking b_scan_clearing #f_scan_marking f_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: false
-    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: false
-    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

config/hybrid_local_planner_params.yaml

@@ -0,0 +1,54 @@
+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

examples/NavigationExample/NavigationAPI.cs

@@ -140,6 +140,10 @@ namespace NavigationExample
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_move_to_order(NavigationHandle handle, Order order, PoseStamped goal);
 
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_move_to_nodes_edges(NavigationHandle handle, IntPtr nodes, UIntPtr node_count, IntPtr edges, UIntPtr edge_count, PoseStamped goal);
+
         [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_dock_to(NavigationHandle handle, string marker, PoseStamped goal);

examples/NavigationExample/Program.cs

@@ -405,8 +405,10 @@ namespace NavigationExample
             goal.pose.orientation.w = 1.0;
 
 
-            Console.WriteLine("Docking to docking_point");
+            // Console.WriteLine("Docking to docking_point");
-            NavigationAPI.navigation_dock_to_order(navHandle, order, "charger", goal);
+            // NavigationAPI.navigation_dock_to_order(navHandle, order, "charger", goal);
+
+            NavigationAPI.navigation_move_to_nodes_edges(navHandle, order.nodes, order.nodes_count, order.edges, order.edges_count, goal);
 
             // NavigationAPI.navigation_move_to_order(navHandle, order, goal);
             NavigationAPI.navigation_set_twist_linear(navHandle, 0.1, 0.0, 0.0);
@@ -493,4 +495,3 @@ namespace NavigationExample
         }
     }
 }
-

src/APIs/c_api/include/nav_c_api.h

@@ -141,6 +141,18 @@ extern "C"
    */
   bool navigation_move_to_order(NavigationHandle handle, const Order order, const PoseStamped goal);
 
+  /**
+   * @brief Send a goal for the robot to navigate to
+   * @param handle Navigation handle
+   * @param nodes Nodes array
+   * @param node_count Number of nodes in the array
+   * @param edges Edges array
+   * @param edge_count Number of edges in the array
+   * @param goal Target pose in the global frame
+   * @return true if goal was accepted and sent successfully
+   */
+  bool navigation_move_to_nodes_edges(NavigationHandle handle, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal);
+
   /**
    * @brief Send a docking goal to a predefined marker
    * @param handle Navigation handle

src/APIs/c_api/src/nav_c_api.cpp

@@ -171,6 +171,7 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po
 
   try
   {
+
     std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
         std::shared_ptr<robot::move_base_core::BaseNavigation>(
             reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
@@ -181,10 +182,12 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po
     footprint.reserve(point_count);
     for (size_t i = 0; i < point_count; ++i)
     {
+      
       robot_geometry_msgs::Point pt;
       pt.x = points[i].x;
       pt.y = points[i].y;
       pt.z = points[i].z;
+      printf("footprint x: %f, y: %f\n", pt.x, pt.y);
       footprint.push_back(pt);
     }
     nav_ptr->setRobotFootprint(footprint);
@@ -284,6 +287,40 @@ extern "C" bool navigation_move_to_order(NavigationHandle handle, const Order or
   }
 }
 
+extern "C" bool navigation_move_to_nodes_edges(NavigationHandle handle, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal)
+{
+  if (!handle)
+  {
+    return false;
+  }
+
+  try
+  {
+    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
+        std::shared_ptr<robot::move_base_core::BaseNavigation>(
+            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
+    if (!nav_ptr)
+      return false;
+    robot::log_error("navigation_move_to_order goal %f %f", goal.pose.position.x, goal.pose.position.y);
+    Order order;
+    order.nodes = const_cast<Node *>(nodes);
+    order.nodes_count = node_count;
+    order.edges = const_cast<Edge *>(edges);
+    order.edges_count = edge_count;
+    robot_protocol_msgs::Order cpp_order = convert2CppOrder(order);
+    robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
+    return nav_ptr->moveTo(cpp_order, cpp_goal);
+  }
+  catch (const std::exception &e)
+  {
+    return false;
+  }
+  catch (...)
+  {
+    printf("[%s:%d]\n Error: Failed to move to order\n", __FILE__, __LINE__);
+    return false;
+  }
+}
 extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker, const PoseStamped goal)
 {
   if (!handle || !marker)

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

@@ -1,6 +1,7 @@
 #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;
 
@@ -41,7 +42,19 @@ move_base::ConvertData::~ConvertData()
 // prepare grid_ message for publication.
 void move_base::ConvertData::prepareGrid()
 {
-  // boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
+  printf("Preparing costmap data for publication\n");
+
+  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_;
@@ -53,22 +66,44 @@ 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++)
   {
-    grid_.data[i] = cost_translation_table_[ data[ i ]];
+      for (unsigned int x = 0; x < grid_.info.width; x++)
+      {
+          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()));

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

@@ -760,11 +760,11 @@ void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_na
   //     robot_pose.header.frame_id = robot_base_frame_;
   //     robot_pose.header.stamp = robot::Time();
   //     robot::Time current_time = robot::Time::now(); // save time for checking tf delay later
-      
+        
   //     // Convert robot::Time to tf3::Time
   //     tf3::Time tf3_current_time = data_convert::convertTime(current_time);
   //     tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time());
-      
+        
   //     std::string error_msg;
   //     if (tf_->canTransform("odom", "base_link", tf3_current_time, &error_msg))
   //     {