From ae32077fe24bdbf12c541c2cfff3eafbed27a580 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Thu, 19 Mar 2026 15:24:09 +0700 Subject: [PATCH] update --- .../src/diff/diff_predictive_trajectory.cpp | 4 +- .../Packages/move_base/src/move_base.cpp | 61 ++++++++++++++++++- 2 files changed, 62 insertions(+), 3 deletions(-) diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp index 1a7c0d9..121d050 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp @@ -602,7 +602,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( { // 1) Curvature from pure pursuit const double L2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y; - const double L2_min = 0.05; // m^2, chỉnh theo nhu cầu (0.02–0.1) + const double L2_min = 0.01; // m^2, chỉnh theo nhu cầu (0.02–0.1) const double L2_safe = std::max(L2, L2_min); const double L = std::sqrt(L2_safe); @@ -631,7 +631,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( v_target = std::copysign(min_approach_linear_velocity, sign_x); // 5) Angular speed from curvature - double w_target = v_target * kappa; + double w_target = v_target * kappa + std::copysign(carrot_pose.pose.theta * dt, kappa); if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_) { if (trajectory.poses.size() >= 2) { diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index b67434c..dcec1de 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -31,7 +31,9 @@ #include #include #include -#include +#ifndef BUILD_WITH_ROS + #include +#endif move_base::MoveBase::MoveBase() : initialized_(false), @@ -500,6 +502,7 @@ robot_nav_msgs::OccupancyGrid move_base::MoveBase::getStaticMap(const std::strin void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) { auto it = laser_scans_.find(laser_scan_name); +#ifndef BUILD_WITH_ROS laser_filter::LaserScanSOR sor; sor.setMeanK(10); // xét 10 điểm láng giềng gần nhất sor.setStddevMulThresh(1.0); // ngưỡng = mean + 1.0 * stddev @@ -512,6 +515,16 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot { it->second = laser_scan_filter; } +#else + if (it == laser_scans_.end()) + { + laser_scans_[laser_scan_name] = laser_scan; + } + else + { + it->second = laser_scan; + } +#endif // robot::log_info("stamp: %ld.%ld", laser_scan.header.stamp.sec, laser_scan.header.stamp.nsec); // robot::log_info("frame_id: %s", laser_scan.header.frame_id.c_str()); // robot::log_info("angle_min: %f", laser_scan.angle_min); @@ -534,8 +547,13 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot // } // robot::log_error("intensities: %s", intensities_str.str().c_str()); +#ifndef BUILD_WITH_ROS updateLocalCostmap(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); updateGlobalCostmap(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); +#else + updateLocalCostmap(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); + updateGlobalCostmap(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); +#endif } robot_sensor_msgs::LaserScan move_base::MoveBase::getLaserScan(const std::string &laser_scan_name) @@ -1225,6 +1243,47 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry } if (cancel_ctr_) cancel_ctr_ = false; + // Check if action server exists + if (!as_) + { + robot::log_error("[MoveBase::moveTo] as_ pointer is null!"); + lock.unlock(); + return false; + } + + try + { + robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared(); + action_goal->header.stamp = robot::Time::now(); + action_goal->goal.target_pose = goal; + + // Generate unique goal ID using timestamp + robot::Time now = robot::Time::now(); + action_goal->goal_id.stamp = now; + std::ostringstream goal_id_stream; + goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec; + action_goal->goal_id.id = goal_id_stream.str(); + + robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str()); + robot::log_info("[MoveBase::moveTo] Goal stamp: %ld.%09ld", + action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec); + + // Clear Order message since this is a non-Order moveTo call + { + boost::unique_lock planner_lock(planner_mutex_); + planner_order_.reset(); + } + + robot::log_info("[MoveBase::moveTo] Processing goal through action server..."); + as_->processGoal(action_goal); + robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server"); + } + catch (const std::exception &e) + { + lock.unlock(); + robot::log_error("[MoveBase::moveTo] Exception during processGoal: %s", e.what()); + return false; + } lock.unlock(); return true;