Merge remote-tracking branch 'origin/3.0' into awc-devel

.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

@@ -19,17 +19,33 @@ virtual_walls_map:
   lethal_cost_threshold: 100
 
 obstacles:
-  observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing 
-  f_scan_marking:
-    topic: /f_scan
+  observation_sources:  l_scan_marking l_scan_clearing r_scan_marking r_scan_clearing b_scan_marking b_scan_clearing
+  l_scan_marking:
+    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:
-    topic: /f_scan
+  l_scan_clearing:
+    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
@@ -54,3 +70,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.05
+    yaw_goal_tolerance: 0.05
+
+    # 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

config/maker_sources.yaml

@@ -37,7 +37,7 @@ charger:
     - {name: charger, docking_planner: "DockPlanner", docking_nav: ""}
 
   charger:
-    maker_goal_frame: charger_goal
+    maker_goal_frame: charger
     footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
     delay: 1.5 # Cấm sửa không là không chạy được
     timeout: 60

config/pnkx_local_planner_params.yaml

@@ -48,7 +48,7 @@ LimitedAccelGenerator:
   min_vel_y: 0.0  # diff drive robot
 
   max_speed_xy: 2.0    # max_trans_vel: 0.8  # choose slightly less than the base's capability
-  min_speed_xy: 0.25    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
+  min_speed_xy: 0.15    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
 
   max_vel_theta: 0.4   # max_rot_vel: 1.0  # choose slightly less than the base's capability
   min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity

src/APIs/c_api/src/nav_c_api.cpp

@@ -572,7 +572,6 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
     linear.x = linear_x;
     linear.y = linear_y;
     linear.z = linear_z;
-    robot::log_info("setTwistLinear %f", linear.x);
     return nav_ptr->setTwistLinear(linear);
   }
   catch (...)

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp

@@ -630,51 +630,49 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   if (std::fabs(v_target) < min_approach_linear_velocity)
     v_target = std::copysign(min_approach_linear_velocity, sign_x);
 
-  std::stringstream ss;
-
   // 5) Angular speed from curvature
   double w_target = v_target * kappa;
   if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
   {
-    ss << w_target << " ";
-    if (trajectory.poses.size() >= 2) {
-      const auto& p1 = trajectory.poses[trajectory.poses.size() - 1].pose;
-      double heading_ref = 0.0;
-      for(int i = trajectory.poses.size() - 2; i >= 0; i--)
-      {
-        const auto& p = trajectory.poses[i].pose;
-        const auto& dx = sign_x < 0.0 ? p1.x - p.x : p.x - p1.x;
-        const auto& dy = sign_x < 0.0 ? p1.y - p.y : p.y - p1.y;
-        if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution())
-        {
-          if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
-            continue;
-          heading_ref = angles::normalize_angle(std::atan2(dy, dx));
-          break;
-        }
-      }
+    // ss << w_target << " ";
+    // if (trajectory.poses.size() >= 2) {
+    //   const auto& p1 = trajectory.poses[trajectory.poses.size() - 1].pose;
+    //   double heading_ref = 0.0;
+    //   for(int i = trajectory.poses.size() - 2; i >= 0; i--)
+    //   {
+    //     const auto& p = trajectory.poses[i].pose;
+    //     const auto& dx = sign_x < 0.0 ? p1.x - p.x : p.x - p1.x;
+    //     const auto& dy = sign_x < 0.0 ? p1.y - p.y : p.y - p1.y;
+    //     if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution())
+    //     {
+    //       if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
+    //         continue;
+    //       heading_ref = angles::normalize_angle(std::atan2(dy, dx));
+    //       break;
+    //     }
+    //   }
 
-      const double error = angles::normalize_angle(heading_ref);
-      ss << "error: " << error << " ";
-      double w_heading = 0.0;
-      pid(error, 
-        near_goal_heading_integral_, 
-        near_goal_heading_last_error_, 
-        dt, 
-        final_heading_kp_angular_, 
-        final_heading_ki_angular_, 
-        final_heading_kd_angular_, 
-        w_heading);
-      // Apply acceleration limits
-      double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
-      w_target = velocity.theta + dw_heading;
-      ss << w_target << " ";
-    }
-    else
-    {
+    //   const double error = angles::normalize_angle(heading_ref);
+    //   ss << "error: " << error << " ";
+    //   double w_heading = 0.0;
+    //   pid(error, 
+    //     near_goal_heading_integral_, 
+    //     near_goal_heading_last_error_, 
+    //     dt, 
+    //     final_heading_kp_angular_, 
+    //     final_heading_ki_angular_, 
+    //     final_heading_kd_angular_, 
+    //     w_heading);
+    //   // Apply acceleration limits
+    //   double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
+    //   w_target = velocity.theta + dw_heading;
+    //   ss << w_target << " ";
+    // }
+    // else
+    // {
       w_target = 0.0;
       near_goal_heading_was_active_ = false;
-    }
+    // }
   }
   else
   {
@@ -689,8 +687,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 
   drive_cmd.x = velocity.x + dv;
   drive_cmd.theta = velocity.theta + dw;
-
-  ss  << drive_cmd.theta << " ";
   Eigen::VectorXd y(2);
   y << drive_cmd.x, drive_cmd.theta;
 
@@ -710,7 +706,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
   if (kf_filter_angular_)
     drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
-  ROS_WARN_THROTTLE(0.1, "%s", ss.str().c_str());
 }
 
 void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -1241,7 +1236,7 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
 
   if (max_kappa <= straight_threshold && fabs(path.poses.back().pose.x) < min_lookahead_dist_) // nếu đường thẳng
   { 
-    if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.back().pose.x) < (min_lookahead_dist_ + max_path_distance_))
+    if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.back().pose.x) < min_lookahead_dist_)
     {
       return generateParallelPath(path, sign_x);
     }

src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp

@@ -194,6 +194,10 @@ namespace two_points_planner
       pose.pose.position.x += resolution * cos(theta);
       pose.pose.position.y += resolution * sin(theta);
       plan.push_back(pose);
+      pose = start;
+      pose.pose.position.x -= resolution * cos(theta);
+      pose.pose.position.y -= resolution * sin(theta);
+      plan.push_back(pose);
       plan.push_back(goal);
       return true;
     }

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp

@@ -255,7 +255,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
 
   parent_.setParam(algorithm_nav_name, original_papams_);
   robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
-  nh_algorithm.setParam("allow_rotate", false);
+  // nh_algorithm.setParam("allow_rotate", false);
 }
 
 void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
@@ -453,7 +453,8 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_
     }
   }
 
-  if (ret_nav_ && !ret_angle_ && !dock_ok)
+  // if (ret_nav_ && !ret_angle_ && !dock_ok)
+  if (ret_nav_ && !ret_angle_)
   {
     double delta_orient =
         fabs(angles::normalize_angle(local_goal.pose.theta - local_pose.pose.theta));

src/Libraries/robot_cpp/src/plugin_loader_helper.cpp

@@ -66,7 +66,6 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name)
   }
   // Try to read from NodeHandle
   std::string library_path;
-  robot::log_info_at(__FILE__, __LINE__, "%s", symbol_name.c_str());
   if (nh_.hasParam(param_path)) {
     nh_.getParam(param_path, library_path, std::string(""));
     if (!library_path.empty()) {
@@ -344,7 +343,7 @@ std::string PluginLoaderHelper::getBuildDirectory()
 std::string PluginLoaderHelper::getWorkspacePath()
 {
   // Method 1: Từ environment variable PNKX_NAV_CORE_DIR
-  const char* workspace_path = std::getenv("PNKX_NAV_CORE_LIBRARY_PATH");
+  const char* workspace_path = std::getenv("PNKX_NAV_CORE_DIR");
   if (workspace_path && std::filesystem::exists(workspace_path)) {
     return std::string(workspace_path);
   }

src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h

@@ -146,7 +146,7 @@ namespace robot_nav_core
      * @brief Gets the current global plan
      * @param path The global plan
      */
-    virtual void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) = 0;
+    virtual void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) {return;}
 
     /**
      * @brief  Constructs the local planner

src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h

@@ -103,7 +103,7 @@ namespace robot_nav_core2
      * @brief Gets the current global plan
      * @param path The global plan
      */
-    virtual void getGlobalPlan(robot_nav_2d_msgs::Path2D &path) = 0;
+    virtual void getGlobalPlan(robot_nav_2d_msgs::Path2D &path) {return;}
 
     /**
      * @brief Compute the best command given the current pose, velocity and goal

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

@@ -977,11 +977,6 @@ bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal,
     robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
     as_->processGoal(action_goal);
 
-    setTwistLinear(old_linear_fwd_);
-    setTwistLinear(old_linear_bwd_);
-    setTwistAngular(old_angular_fwd_);
-    setTwistAngular(old_angular_rev_);
-
     robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
   }
   catch (const std::exception &e)
@@ -1122,13 +1117,7 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
       lock.unlock();
       return false;
     }
-
     as_->processGoal(action_goal);
-
-    setTwistLinear(old_linear_fwd_);
-    setTwistLinear(old_linear_bwd_);
-    setTwistAngular(old_angular_fwd_);
-    setTwistAngular(old_angular_rev_);
   }
   catch (const std::exception &e)
   {
@@ -1289,11 +1278,6 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry
     }
     
     robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
-    as_->processGoal(action_goal);
-    setTwistLinear(old_linear_fwd_);
-    setTwistLinear(old_linear_bwd_);
-    setTwistAngular(old_angular_fwd_);
-    setTwistAngular(old_angular_rev_);
     robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
   }
   catch (const std::exception &e)
@@ -1440,10 +1424,6 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
       return false;
     }
     as_->processGoal(action_goal);
-    setTwistLinear(old_linear_fwd_);
-    setTwistLinear(old_linear_bwd_);
-    setTwistAngular(old_angular_fwd_);
-    setTwistAngular(old_angular_rev_);
   }
   catch (const std::exception &e)
   {