HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix move base

Browse Source
This commit is contained in:
QUYVN
2026-03-22 08:57:09 +00:00
parent 5583b3e0f2
commit d0ad2d0e21
2 changed files with 15 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp
View File

@@ -174,8 +174,9 @@ namespace two_points_planner
// Tính toán khoảng cách giữa điểm bắt đầu và điểm đích
const double dx = goal.pose.position.x - start.pose.position.x;
const double dy = goal.pose.position.y - start.pose.position.y;
double distance = std::sqrt(dx * dx + dy * dy);
const double distance = std::sqrt(dx * dx + dy * dy);
double theta;
if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
{
theta = std::atan2(dy, dx);
@@ -186,9 +187,13 @@ namespace two_points_planner
if(cos(goal_theta - theta) < 0) theta += M_PI;
}
else
{
robot::log_error("[%s:%d]\n TwoPointsPlanner: can not calculating theta from 'start point' or 'goal point'", __FILE__, __LINE__);
return false;
{
robot_geometry_msgs::PoseStamped pose = start;
pose.pose.position.x += 0.01 * cos(theta);
pose.pose.position.y += 0.01 * sin(theta);
plan.push_back(pose);
plan.push_back(goal);
return true;
}
// Lấy độ phân giải của costmap

9
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@@ -271,6 +271,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
{
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(global_planner);
robot::log_info("[%s:%d]\n INFO: global_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
path_file_so, global_planner, boost::dll::load_mode::append_decorations);
@@ -314,6 +315,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
{
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(local_planner);
robot::log_info("[%s:%d]\n INFO: local_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
controller_loader_ =
boost::dll::import_alias<robot_nav_core::BaseLocalPlanner::Ptr()>(
path_file_so, local_planner, boost::dll::load_mode::append_decorations);
@@ -401,7 +403,7 @@ void move_base::MoveBase::swapPlanner(std::string base_global_planner)
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(base_global_planner);
robot::log_info("[%s:%d]\n INFO: global_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
auto new_loader = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
path_file_so, base_global_planner, boost::dll::load_mode::append_decorations);
auto new_planner = new_loader();
@@ -2005,6 +2007,7 @@ bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node)
std::string behavior_name = behavior["name"].as<std::string>();
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(behavior_type);
robot::log_info("Loading recovery behavior '%s' of type '%s' from '%s'", behavior_name.c_str(), behavior_type.c_str(), path_file_so.c_str());
// Load the factory function from the shared library
recovery_loaders_[behavior_name] = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
path_file_so, behavior_type, boost::dll::load_mode::append_decorations);
@@ -2864,7 +2867,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
{
robot::log_debug("Goal reached!");
resetState();
swapPlanner(default_config_.base_global_planner);
// swapPlanner(default_config_.base_global_planner);
// disable the planner thread
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
runPlanner_ = false;
@@ -3070,7 +3073,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
{
robot::log_debug("All recovery behaviors have failed, locking the planner and disabling it.");
resetState();
swapPlanner(default_config_.base_global_planner);
// swapPlanner(default_config_.base_global_planner);
// disable the planner thread
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);