add dock to

.gitignore

@@ -422,3 +422,4 @@ build
 install
 devel
 
+obstacle

CMakeLists.txt

@@ -138,6 +138,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/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,17 +27,33 @@ virtual_walls_map:
   lethal_cost_threshold: 100
 
 obstacles:
-  observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing 
+  observation_sources:  l_scan_marking l_scan_clearing r_scan_marking r_scan_clearing b_scan_marking b_scan_clearing
-  f_scan_marking:
+  l_scan_marking:
-    topic: /f_scan
+    topic: /l_scan
     data_type: LaserScan
     clearing: false
     marking: true
     inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
-  f_scan_clearing:
+  l_scan_clearing:
-    topic: /f_scan
+    topic: /l_scan
+    data_type: LaserScan
+    clearing: true
+    marking: false
+    inf_is_valid: true
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  r_scan_marking:
+    topic: /r_scan
+    data_type: LaserScan
+    clearing: false
+    marking: true
+    inf_is_valid: true
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  r_scan_clearing:
+    topic: /r_scan
     data_type: LaserScan
     clearing: true
     marking: false
@@ -61,3 +78,4 @@ obstacles:
     max_obstacle_height: 0.25
   
 
+

config/costmap_global_params.yaml

@@ -4,7 +4,7 @@ global_costmap:
   global_frame: map
   update_frequency: 1.0
   publish_frequency: 1.0
-  raytrace_range: 2.0
+  raytrace_range: 3.5
   resolution: 0.05
   z_resolution: 0.2
   rolling_window: false

config/costmap_local_params.yaml

@@ -5,7 +5,7 @@ local_costmap:
   update_frequency: 6.0
   publish_frequency: 6.0
   rolling_window: true
-  raytrace_range: 2.0
+  raytrace_range: 3.5
   resolution: 0.05
   z_resolution: 0.15
   z_voxels: 8

config/hybrid_local_planner_params.yaml

@@ -0,0 +1,59 @@
+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
+
+    robot_max_v_ac: 0.4
+    robot_max_w_ac: 0.4
+    robot_max_v_pt: 1.0
+    robot_max_w_pt: 0.4
+    robot_max_v_backwards_pt: -0.2
+    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.1
+    yaw_goal_tolerance: 0.07
+
+    # Optimization
+
+    # PP Parameters
+    w_vel: 0.8
+    w_omega: 1.0
+    # DWA Parameters
+    enable_backward_motion: false
+    w_targetheading_ac: 1.7
+    w_velocity_ac: 0.2
+    w_clearance_ac: 0.2
+    w_pathDistance_ac: 0.05
+    w_smoothness_ac: 0.3
+    w_targetheading_pt: 0.2
+    w_velocity_pt: 0.8
+    w_clearance_pt: 0.1
+    w_pathDistance_pt: 2.1
+    w_smoothness_pt: 0.3
+    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/APIs/c_api/include/nav_c_api.h

@@ -171,6 +171,20 @@ extern "C"
    */
   bool navigation_dock_to_order(NavigationHandle handle, const Order order, const char *marker, const PoseStamped goal);
 
+    /**
+   * @brief Send a goal for the robot to navigate to
+   * @param handle Navigation handle
+   * @param marker Marker name or ID (null-terminated string)
+   * @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_dock_to_nodes_edges(NavigationHandle handle, const char *marker, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal);
+
+
   /**
    * @brief Move straight toward the target position
    * @param handle Navigation handle

src/APIs/c_api/src/nav_c_api.cpp

@@ -397,6 +397,44 @@ extern "C" bool navigation_dock_to_order(NavigationHandle handle, const Order or
   }
 }
 
+extern "C" bool navigation_dock_to_nodes_edges(NavigationHandle handle, const char *marker, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal)
+{
+  if (!handle || !marker)
+  {
+    return false;
+  }
+  if (!isQuaternionValid(goal.pose.orientation))
+  {
+    robot::log_error("Goal quaternion is invalid");
+    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;
+    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->dockTo(cpp_order, std::string(marker), cpp_goal);
+  }
+  catch (const std::exception &e)
+  {
+    return false;
+  }
+  catch (...)
+  {
+    printf("[%s:%d]\n Error: Failed to dock to nodes edges\n", __FILE__, __LINE__);
+    return false;
+  }
+}
+
 extern "C" bool navigation_move_straight_to(NavigationHandle handle, const double distance)
 {
   if (!handle)

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

@@ -812,7 +812,17 @@ bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal,
 {
   if (setup_)
   {
-    swapPlanner(default_config_.base_global_planner);
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
+    {
+      swapPlanner(default_config_.base_global_planner);
+    }
   }
   else
   {
@@ -963,7 +973,17 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
 {
   if (setup_)
   {
-    swapPlanner(default_config_.base_global_planner);
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
+    {
+      swapPlanner(default_config_.base_global_planner);
+    }
   }
   else
   {
@@ -1120,7 +1140,17 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry
 
   if (setup_)
   {
-    swapPlanner(default_config_.base_global_planner);
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, docking_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
+    {
+      swapPlanner(default_config_.base_global_planner);
+    }
   }
   else
   {
@@ -1233,7 +1263,17 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
 
   if (setup_)
   {
-    swapPlanner(default_config_.base_global_planner);
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, docking_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
+    {
+      swapPlanner(default_config_.base_global_planner);
+    }
   }
   else
   {
@@ -1342,7 +1382,17 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
   if (setup_)
   {
     std::string global_planner = "TwoPointsPlanner";
-    swapPlanner(global_planner);
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, go_straight_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
+    {
+      swapPlanner(global_planner);
+    }
   }
   else
   {
@@ -1465,7 +1515,17 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
   if (setup_)
   {
     std::string global_planner = "TwoPointsPlanner";
-    swapPlanner(global_planner);
+    robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, rotate_planner_name_);
+    if(nh_planner.hasParam("base_global_planner"))
+    {
+      std::string base_global_planner;
+      nh_planner.getParam("base_global_planner", base_global_planner);
+      swapPlanner(base_global_planner);
+    }
+    else
+    {
+      swapPlanner(global_planner);
+    }
   }
   else
   {