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

Compare commits

base: HiepLM:2b7cee7642ccc413fd6bfa88036e6cbbf020be8a
Branches Tags
HiepLM:3.0
..
compare: HiepLM:a06beb70b875c1f558b668d9d6a60dc582cf458b
Branches Tags
HiepLM:3.0

2 Commits
2b7cee7642 ... a06beb70b8

Author SHA1 Message Date
HiepLM
a06beb70b8 Refactor move_base initialization and update costmap config 
- Remove automatic initialize() call from MoveBase constructor
- Comment out frame_id and plugins in local costmap config
- Remove unused two_points_global_params.yaml config file
 2025-12-16 09:26:46 +07:00
HiepLM
a7c4b39c6e update module robot_cpp 2025-12-16 09:24:34 +07:00
4 changed files with 176 additions and 174 deletions
Show Stats Expand all files Collapse all files

8
config/costmap_local_params_plugins_no_virtual_walls.yaml
View File

@ -1,8 +1,8 @@
local_costmap: local_costmap:
frame_id: odom # frame_id: odom
plugins: # plugins:
- {name: obstacles, type: "costmap_2d::VoxelLayer" } # - {name: obstacles, type: "costmap_2d::VoxelLayer" }
- {name: inflation, type: "costmap_2d::InflationLayer" } # - {name: inflation, type: "costmap_2d::InflationLayer" }
obstacles: obstacles:
enabled: true enabled: true
footprint_clearing_enabled: true footprint_clearing_enabled: true

3
config1/two_points_global_params.yaml
View File

@ -1,3 +0,0 @@
base_global_planner: TwoPointsPlanner
TwoPointsPlanner:
lethal_obstacle: 20

2
src/Libraries/robot_cpp

@ -1 +1 @@
Subproject commit 2524353bba6dd07114a85a94a6bdff6f9a6fa18c Subproject commit e3df6935432bcfdb14367bc99c2d114c385992a0

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

@ -32,7 +32,6 @@ move_base::MoveBase::MoveBase(TFListenerPtr tf)
runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false), runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
pause_ctr_(false), paused_(false) pause_ctr_(false), paused_(false)
{ {
initialize(tf);
} }
move_base::MoveBase::~MoveBase() move_base::MoveBase::~MoveBase()
@ -70,195 +69,201 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
if (!initialized_) if (!initialized_)
{ {
tf_ = tf; tf_ = tf;
// NodeHandle("~") will automatically load YAML files from config directory // NodeHandle("~") will automatically load YAML files from config directory
robot::NodeHandle nh("~"); robot::NodeHandle nh("~");
private_nh_ = nh; private_nh_ = nh;
recovery_trigger_ = PLANNING_R; recovery_trigger_ = PLANNING_R;
// get some parameters that will be global to the move base node robot::NodeHandle nh1 = robot::NodeHandle(nh);
printf("[%s:%d] Getting planner parameters\n", __FILE__, __LINE__); nh1.setParam("local_costmap/obstacles/enabled", false);
std::string global_planner, local_planner; // nh.printAllParams();
private_nh_.getParam("base_global_planner", global_planner, std::string("")); // nh1.printAllParams();
printf("[%s:%d] global_planner: %s\n", __FILE__, __LINE__, global_planner.c_str()); // nh.printAllParams();
private_nh_.getParam("base_local_planner", local_planner, std::string(""));
printf("[%s:%d] local_planner: %s\n", __FILE__, __LINE__, local_planner.c_str());
// Handle nested YAML nodes for costmap config // // get some parameters that will be global to the move base node
std::string robot_base_frame; // std::string global_planner, local_planner;
private_nh_.getParam("robot_base_frame", robot_base_frame, std::string("base_footprint")); // private_nh_.getParam("base_global_planner", global_planner, std::string(""));
std::string global_frame; // printf("[%s:%d] global_planner: %s\n", __FILE__, __LINE__, global_planner.c_str());
private_nh_.getParam("global_frame", global_frame, std::string("map")); // private_nh_.getParam("base_local_planner", local_planner, std::string(""));
robot_base_frame_ = robot_base_frame; // printf("[%s:%d] local_planner: %s\n", __FILE__, __LINE__, local_planner.c_str());
global_frame_ = global_frame;
double planner_frequency;
private_nh_.getParam("planner_frequency", planner_frequency, 0.0);
planner_frequency_ = planner_frequency;
double controller_frequency;
private_nh_.getParam("controller_frequency", controller_frequency, 20.0);
controller_frequency_ = controller_frequency;
double planner_patience;
private_nh_.getParam("planner_patience", planner_patience, 5.0);
planner_patience_ = planner_patience;
private_nh_.getParam("controller_patience", controller_patience_, 15.0);
double max_planning_retries;
private_nh_.getParam("max_planning_retries", max_planning_retries, -1.0);
max_planning_retries_ = max_planning_retries;
double oscillation_timeout;
private_nh_.getParam("oscillation_timeout", oscillation_timeout, 0.0);
oscillation_timeout_ = oscillation_timeout;
double oscillation_distance;
private_nh_.getParam("oscillation_distance", oscillation_distance, 0.5);
oscillation_distance_ = oscillation_distance;
double original_xy_goal_tolerance; // // Handle nested YAML nodes for costmap config
private_nh_.getParam("xy_goal_tolerance", original_xy_goal_tolerance, 0.2); // std::string robot_base_frame;
original_xy_goal_tolerance_ = original_xy_goal_tolerance; // private_nh_.getParam("robot_base_frame", robot_base_frame, std::string("base_footprint"));
double original_yaw_goal_tolerance; // std::string global_frame;
private_nh_.getParam("yaw_goal_tolerance", original_yaw_goal_tolerance, 0.2); // private_nh_.getParam("global_frame", global_frame, std::string("map"));
original_yaw_goal_tolerance_ = original_yaw_goal_tolerance; // robot_base_frame_ = robot_base_frame;
// global_frame_ = global_frame;
// double planner_frequency;
// private_nh_.getParam("planner_frequency", planner_frequency, 0.0);
// planner_frequency_ = planner_frequency;
// double controller_frequency;
// private_nh_.getParam("controller_frequency", controller_frequency, 20.0);
// controller_frequency_ = controller_frequency;
// double planner_patience;
// private_nh_.getParam("planner_patience", planner_patience, 5.0);
// planner_patience_ = planner_patience;
// private_nh_.getParam("controller_patience", controller_patience_, 15.0);
// double max_planning_retries;
// private_nh_.getParam("max_planning_retries", max_planning_retries, -1.0);
// max_planning_retries_ = max_planning_retries;
// double oscillation_timeout;
// private_nh_.getParam("oscillation_timeout", oscillation_timeout, 0.0);
// oscillation_timeout_ = oscillation_timeout;
// double oscillation_distance;
// private_nh_.getParam("oscillation_distance", oscillation_distance, 0.5);
// oscillation_distance_ = oscillation_distance;
// defined local planner name // double original_xy_goal_tolerance;
private_nh_.getParam("position_planner_name", position_planner_name_, std::string("mkt_algorithm/MKTPositionPlanner")); // private_nh_.getParam("xy_goal_tolerance", original_xy_goal_tolerance, 0.2);
private_nh_.getParam("docking_planner_name", docking_planner_name_, std::string("mkt_algorithm/MKTDockingPlanner")); // original_xy_goal_tolerance_ = original_xy_goal_tolerance;
private_nh_.getParam("go_straight_planner_name", go_straight_planner_name_, std::string("mkt_algorithm/MKTGoStraightPlanner")); // double original_yaw_goal_tolerance;
private_nh_.getParam("rotate_planner_name", rotate_planner_name_, std::string("mkt_algorithm/MKTRotatePlanner")); // private_nh_.getParam("yaw_goal_tolerance", original_yaw_goal_tolerance, 0.2);
// original_yaw_goal_tolerance_ = original_yaw_goal_tolerance;
// parameters of make_plan service // // defined local planner name
private_nh_.getParam("make_plan_clear_costmap", make_plan_clear_costmap_, false); // private_nh_.getParam("position_planner_name", position_planner_name_, std::string("mkt_algorithm/MKTPositionPlanner"));
private_nh_.getParam("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, false); // private_nh_.getParam("docking_planner_name", docking_planner_name_, std::string("mkt_algorithm/MKTDockingPlanner"));
private_nh_.getParam("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0); // private_nh_.getParam("go_straight_planner_name", go_straight_planner_name_, std::string("mkt_algorithm/MKTGoStraightPlanner"));
min_approach_linear_velocity_ *= 1.2; // private_nh_.getParam("rotate_planner_name", rotate_planner_name_, std::string("mkt_algorithm/MKTRotatePlanner"));
// set up plan triple buffer
planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
// set up the planner's thread // // parameters of make_plan service
planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this)); // private_nh_.getParam("make_plan_clear_costmap", make_plan_clear_costmap_, false);
// private_nh_.getParam("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, false);
// private_nh_.getParam("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
// min_approach_linear_velocity_ *= 1.2;
// // set up plan triple buffer
// planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
// latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
// controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
// we'll assume the radius of the robot to be consistent with what's specified for the costmaps // // set up the planner's thread
// From config param // planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this));
double inscribed_radius;
private_nh_.getParam("local_costmap/inscribed_radius", inscribed_radius, 0.2);
double circumscribed_radius;
private_nh_.getParam("local_costmap/circumscribed_radius", circumscribed_radius, 0.3);
inscribed_radius_ = inscribed_radius;
circumscribed_radius_ = circumscribed_radius;
private_nh_.getParam("clearing_radius", clearing_radius_, 0.0);
double conservative_reset_dist;
private_nh_.getParam("conservative_reset_dist", conservative_reset_dist, 0.0);
conservative_reset_dist_ = conservative_reset_dist;
private_nh_.getParam("shutdown_costmaps", shutdown_costmaps_, false);
private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true);
private_nh_.getParam("recovery_behavior_enabled", recovery_behavior_enabled_, true);
// create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map // // we'll assume the radius of the robot to be consistent with what's specified for the costmaps
planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_); // // From config param
planner_costmap_robot_->pause(); // double inscribed_radius;
// initialize the global planner // private_nh_.getParam("local_costmap/inscribed_radius", inscribed_radius, 0.2);
try // double circumscribed_radius;
{ // private_nh_.getParam("local_costmap/circumscribed_radius", circumscribed_radius, 0.3);
planner_loader_ = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>( // inscribed_radius_ = inscribed_radius;
"/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/global_planners/two_points_planner/libtwo_points_planner.so", // circumscribed_radius_ = circumscribed_radius;
global_planner, // private_nh_.getParam("clearing_radius", clearing_radius_, 0.0);
boost::dll::load_mode::append_decorations); // double conservative_reset_dist;
// private_nh_.getParam("conservative_reset_dist", conservative_reset_dist, 0.0);
// conservative_reset_dist_ = conservative_reset_dist;
// private_nh_.getParam("shutdown_costmaps", shutdown_costmaps_, false);
// private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true);
// private_nh_.getParam("recovery_behavior_enabled", recovery_behavior_enabled_, true);
planner_ = planner_loader_(); // // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
if (!planner_) // planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
{ // planner_costmap_robot_->pause();
robot::printf_red("[%s:%d] ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__); // // initialize the global planner
throw std::runtime_error("Failed to load global planner " + global_planner); // try
} // {
if(planner_->initialize(global_planner, planner_costmap_robot_)) // planner_loader_ = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>(
printf("[%s:%d] Global planner initialized successfully\n", __FILE__, __LINE__); // "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/global_planners/two_points_planner/libtwo_points_planner.so",
else // global_planner,
robot::printf_red("[%s:%d] Global planner initialized failed\n", __FILE__, __LINE__); // boost::dll::load_mode::append_decorations);
}
catch (const std::exception &ex)
{
printf("[%s:%d] EXCEPTION in global planner: %s\n", __FILE__, __LINE__, ex.what());
throw std::runtime_error("Failed to create the " + global_planner + " planner");
}
// create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
controller_costmap_robot_ = new costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
controller_costmap_robot_->pause();
// create a local planner
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Cores/nav_core_adapter/liblocal_planner_adapter.so";
controller_loader_ =
boost::dll::import_alias<nav_core::BaseLocalPlanner::Ptr()>(
path_file_so, "LocalPlannerAdapter", boost::dll::load_mode::append_decorations);
tc_ = controller_loader_();
if (!tc_)
{
robot::printf_red("[%s:%d] ERROR: controller_loader_() returned nullptr\n", __FILE__, __LINE__);
throw std::runtime_error("Failed to load local planner " + local_planner);
}
// tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_);
}
catch (const std::exception &ex)
{
printf("[%s:%d] EXCEPTION in local planner: %s\n", __FILE__, __LINE__, ex.what());
throw std::runtime_error("Failed to create the " + local_planner + " planner");
}
// Start actively updating costmaps based on sensor data // planner_ = planner_loader_();
planner_costmap_robot_->start(); // if (!planner_)
controller_costmap_robot_->start(); // {
// robot::printf_red("[%s:%d] ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__);
// throw std::runtime_error("Failed to load global planner " + global_planner);
// }
// if(planner_->initialize(global_planner, planner_costmap_robot_))
// printf("[%s:%d] Global planner initialized successfully\n", __FILE__, __LINE__);
// else
// robot::printf_red("[%s:%d] Global planner initialized failed\n", __FILE__, __LINE__);
// }
// catch (const std::exception &ex)
// {
// printf("[%s:%d] EXCEPTION in global planner: %s\n", __FILE__, __LINE__, ex.what());
// throw std::runtime_error("Failed to create the " + global_planner + " planner");
// }
// // create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
// controller_costmap_robot_ = new costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
// controller_costmap_robot_->pause();
// // create a local planner
// try
// {
// std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Cores/nav_core_adapter/liblocal_planner_adapter.so";
// controller_loader_ =
// boost::dll::import_alias<nav_core::BaseLocalPlanner::Ptr()>(
// path_file_so, "LocalPlannerAdapter", boost::dll::load_mode::append_decorations);
// tc_ = controller_loader_();
// if (!tc_)
// {
// robot::printf_red("[%s:%d] ERROR: controller_loader_() returned nullptr\n", __FILE__, __LINE__);
// throw std::runtime_error("Failed to load local planner " + local_planner);
// }
// // tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_);
// }
// catch (const std::exception &ex)
// {
// printf("[%s:%d] EXCEPTION in local planner: %s\n", __FILE__, __LINE__, ex.what());
// throw std::runtime_error("Failed to create the " + local_planner + " planner");
// }
// // Start actively updating costmaps based on sensor data
// planner_costmap_robot_->start();
// controller_costmap_robot_->start();
try // try
{ // {
old_linear_fwd_ = tc_->getTwistLinear(true); // old_linear_fwd_ = tc_->getTwistLinear(true);
old_linear_bwd_ = tc_->getTwistLinear(false); // old_linear_bwd_ = tc_->getTwistLinear(false);
old_angular_fwd_ = tc_->getTwistAngular(true); // old_angular_fwd_ = tc_->getTwistAngular(true);
old_angular_rev_ = tc_->getTwistAngular(false); // old_angular_rev_ = tc_->getTwistAngular(false);
} // }
catch (const std::exception &e) // catch (const std::exception &e)
{ // {
std::cerr << e.what() << '\n'; // std::cerr << e.what() << '\n';
} // }
// // advertise a service for getting a plan // // // advertise a service for getting a plan
// make_plan_srv_ = private_nh.advertiseService("make_plan", &move_base::MoveBase::planService, this); // // make_plan_srv_ = private_nh.advertiseService("make_plan", &move_base::MoveBase::planService, this);
// // advertise a service for clearing the costmaps // // // advertise a service for clearing the costmaps
// clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &move_base::MoveBase::clearCostmapsService, this); // // clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &move_base::MoveBase::clearCostmapsService, this);
// if we shutdown our costmaps when we're deactivated... we'll do that now // // if we shutdown our costmaps when we're deactivated... we'll do that now
if (shutdown_costmaps_) // if (shutdown_costmaps_)
{ // {
planner_costmap_robot_->stop(); // planner_costmap_robot_->stop();
controller_costmap_robot_->stop(); // controller_costmap_robot_->stop();
} // }
// load any user specified recovery behaviors, and if that fails load the defaults // // load any user specified recovery behaviors, and if that fails load the defaults
if (!loadRecoveryBehaviors(private_nh_)) // if (!loadRecoveryBehaviors(private_nh_))
{ // {
robot::printf_yellow("[%s:%d] Loading default recovery behaviors\n", __FILE__, __LINE__); // robot::printf_yellow("[%s:%d] Loading default recovery behaviors\n", __FILE__, __LINE__);
loadDefaultRecoveryBehaviors(); // loadDefaultRecoveryBehaviors();
} // }
// initially, we'll need to make a plan // // initially, we'll need to make a plan
state_ = move_base::PLANNING; // state_ = move_base::PLANNING;
// we'll start executing recovery behaviors at the beginning of our list // // we'll start executing recovery behaviors at the beginning of our list
recovery_index_ = 0; // recovery_index_ = 0;
nav_feedback_ = std::make_shared<move_base_core::NavFeedback>(); // nav_feedback_ = std::make_shared<move_base_core::NavFeedback>();
if (nav_feedback_) // if (nav_feedback_)
{ // {
nav_feedback_->navigation_state = move_base_core::State::PLANNING; // nav_feedback_->navigation_state = move_base_core::State::PLANNING;
nav_feedback_->is_ready = true; // nav_feedback_->is_ready = true;
} // }
else // else
{ // {
robot::printf_red("[%s:%d] ERROR: nav_feedback_ is nullptr\n", __FILE__, __LINE__); // robot::printf_red("[%s:%d] ERROR: nav_feedback_ is nullptr\n", __FILE__, __LINE__);
} // }
initialized_ = true; // initialized_ = true;
printf("[%s:%d] ========== End: initialize() - SUCCESS ==========\n", __FILE__, __LINE__); // printf("[%s:%d] ========== End: initialize() - SUCCESS ==========\n", __FILE__, __LINE__);
} }
else else
{ {