From a06beb70b875c1f558b668d9d6a60dc582cf458b Mon Sep 17 00:00:00 2001 From: HiepLM Date: Tue, 16 Dec 2025 09:26:46 +0700 Subject: [PATCH] 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 --- ...local_params_plugins_no_virtual_walls.yaml | 8 +- config1/two_points_global_params.yaml | 3 - .../Packages/move_base/src/move_base.cpp | 337 +++++++++--------- 3 files changed, 175 insertions(+), 173 deletions(-) delete mode 100644 config1/two_points_global_params.yaml diff --git a/config/costmap_local_params_plugins_no_virtual_walls.yaml b/config/costmap_local_params_plugins_no_virtual_walls.yaml index 6ef480b..a45b181 100755 --- a/config/costmap_local_params_plugins_no_virtual_walls.yaml +++ b/config/costmap_local_params_plugins_no_virtual_walls.yaml @@ -1,8 +1,8 @@ local_costmap: - frame_id: odom - plugins: - - {name: obstacles, type: "costmap_2d::VoxelLayer" } - - {name: inflation, type: "costmap_2d::InflationLayer" } + # frame_id: odom + # plugins: + # - {name: obstacles, type: "costmap_2d::VoxelLayer" } + # - {name: inflation, type: "costmap_2d::InflationLayer" } obstacles: enabled: true footprint_clearing_enabled: true diff --git a/config1/two_points_global_params.yaml b/config1/two_points_global_params.yaml deleted file mode 100644 index ca0b60f..0000000 --- a/config1/two_points_global_params.yaml +++ /dev/null @@ -1,3 +0,0 @@ -base_global_planner: TwoPointsPlanner -TwoPointsPlanner: - lethal_obstacle: 20 \ No newline at end of file diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 2f8b84f..7bfaa78 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -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), pause_ctr_(false), paused_(false) { - initialize(tf); } move_base::MoveBase::~MoveBase() @@ -70,195 +69,201 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) if (!initialized_) { tf_ = tf; + // NodeHandle("~") will automatically load YAML files from config directory robot::NodeHandle nh("~"); private_nh_ = nh; recovery_trigger_ = PLANNING_R; + + robot::NodeHandle nh1 = robot::NodeHandle(nh); + nh1.setParam("local_costmap/obstacles/enabled", false); + // nh.printAllParams(); + // nh1.printAllParams(); + // nh.printAllParams(); - // get some parameters that will be global to the move base node - printf("[%s:%d] Getting planner parameters\n", __FILE__, __LINE__); - std::string global_planner, local_planner; - private_nh_.getParam("base_global_planner", global_planner, std::string("")); - printf("[%s:%d] global_planner: %s\n", __FILE__, __LINE__, global_planner.c_str()); - private_nh_.getParam("base_local_planner", local_planner, std::string("")); - printf("[%s:%d] local_planner: %s\n", __FILE__, __LINE__, local_planner.c_str()); + // // get some parameters that will be global to the move base node + // std::string global_planner, local_planner; + // private_nh_.getParam("base_global_planner", global_planner, std::string("")); + // printf("[%s:%d] global_planner: %s\n", __FILE__, __LINE__, global_planner.c_str()); + // 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 - std::string robot_base_frame; - private_nh_.getParam("robot_base_frame", robot_base_frame, std::string("base_footprint")); - std::string global_frame; - private_nh_.getParam("global_frame", global_frame, std::string("map")); - 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; + // // Handle nested YAML nodes for costmap config + // std::string robot_base_frame; + // private_nh_.getParam("robot_base_frame", robot_base_frame, std::string("base_footprint")); + // std::string global_frame; + // private_nh_.getParam("global_frame", global_frame, std::string("map")); + // 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; - double original_xy_goal_tolerance; - private_nh_.getParam("xy_goal_tolerance", original_xy_goal_tolerance, 0.2); - original_xy_goal_tolerance_ = original_xy_goal_tolerance; - double original_yaw_goal_tolerance; - private_nh_.getParam("yaw_goal_tolerance", original_yaw_goal_tolerance, 0.2); - original_yaw_goal_tolerance_ = original_yaw_goal_tolerance; + // double original_xy_goal_tolerance; + // private_nh_.getParam("xy_goal_tolerance", original_xy_goal_tolerance, 0.2); + // original_xy_goal_tolerance_ = original_xy_goal_tolerance; + // double original_yaw_goal_tolerance; + // private_nh_.getParam("yaw_goal_tolerance", original_yaw_goal_tolerance, 0.2); + // original_yaw_goal_tolerance_ = original_yaw_goal_tolerance; - // defined local planner name - private_nh_.getParam("position_planner_name", position_planner_name_, std::string("mkt_algorithm/MKTPositionPlanner")); - private_nh_.getParam("docking_planner_name", docking_planner_name_, std::string("mkt_algorithm/MKTDockingPlanner")); - private_nh_.getParam("go_straight_planner_name", go_straight_planner_name_, std::string("mkt_algorithm/MKTGoStraightPlanner")); - private_nh_.getParam("rotate_planner_name", rotate_planner_name_, std::string("mkt_algorithm/MKTRotatePlanner")); + // // defined local planner name + // private_nh_.getParam("position_planner_name", position_planner_name_, std::string("mkt_algorithm/MKTPositionPlanner")); + // private_nh_.getParam("docking_planner_name", docking_planner_name_, std::string("mkt_algorithm/MKTDockingPlanner")); + // private_nh_.getParam("go_straight_planner_name", go_straight_planner_name_, std::string("mkt_algorithm/MKTGoStraightPlanner")); + // private_nh_.getParam("rotate_planner_name", rotate_planner_name_, std::string("mkt_algorithm/MKTRotatePlanner")); - // parameters of make_plan service - 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(); - latest_plan_ = new std::vector(); - controller_plan_ = new std::vector(); + // // parameters of make_plan service + // 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(); + // latest_plan_ = new std::vector(); + // controller_plan_ = new std::vector(); - // set up the planner's thread - planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this)); + // // set up the planner's thread + // planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this)); - // we'll assume the radius of the robot to be consistent with what's specified for the costmaps - // From config param - 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); + // // we'll assume the radius of the robot to be consistent with what's specified for the costmaps + // // From config param + // 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 - planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_); - planner_costmap_robot_->pause(); - // initialize the global planner - try - { - planner_loader_ = boost::dll::import_alias( - "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/global_planners/two_points_planner/libtwo_points_planner.so", - global_planner, - boost::dll::load_mode::append_decorations); + // // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map + // planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_); + // planner_costmap_robot_->pause(); + // // initialize the global planner + // try + // { + // planner_loader_ = boost::dll::import_alias( + // "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/global_planners/two_points_planner/libtwo_points_planner.so", + // global_planner, + // boost::dll::load_mode::append_decorations); - planner_ = planner_loader_(); - if (!planner_) - { - 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( - 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"); - } + // planner_ = planner_loader_(); + // if (!planner_) + // { + // 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( + // 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(); + // // Start actively updating costmaps based on sensor data + // planner_costmap_robot_->start(); + // controller_costmap_robot_->start(); - try - { - old_linear_fwd_ = tc_->getTwistLinear(true); - old_linear_bwd_ = tc_->getTwistLinear(false); + // try + // { + // old_linear_fwd_ = tc_->getTwistLinear(true); + // old_linear_bwd_ = tc_->getTwistLinear(false); - old_angular_fwd_ = tc_->getTwistAngular(true); - old_angular_rev_ = tc_->getTwistAngular(false); - } - catch (const std::exception &e) - { - std::cerr << e.what() << '\n'; - } + // old_angular_fwd_ = tc_->getTwistAngular(true); + // old_angular_rev_ = tc_->getTwistAngular(false); + // } + // catch (const std::exception &e) + // { + // std::cerr << e.what() << '\n'; + // } - // // advertise a service for getting a plan - // make_plan_srv_ = private_nh.advertiseService("make_plan", &move_base::MoveBase::planService, this); + // // // advertise a service for getting a plan + // // make_plan_srv_ = private_nh.advertiseService("make_plan", &move_base::MoveBase::planService, this); - // // advertise a service for clearing the costmaps - // clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &move_base::MoveBase::clearCostmapsService, this); + // // // advertise a service for clearing the costmaps + // // 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 (shutdown_costmaps_) - { - planner_costmap_robot_->stop(); - controller_costmap_robot_->stop(); - } + // // if we shutdown our costmaps when we're deactivated... we'll do that now + // if (shutdown_costmaps_) + // { + // planner_costmap_robot_->stop(); + // controller_costmap_robot_->stop(); + // } - // load any user specified recovery behaviors, and if that fails load the defaults - if (!loadRecoveryBehaviors(private_nh_)) - { - robot::printf_yellow("[%s:%d] Loading default recovery behaviors\n", __FILE__, __LINE__); - loadDefaultRecoveryBehaviors(); - } + // // load any user specified recovery behaviors, and if that fails load the defaults + // if (!loadRecoveryBehaviors(private_nh_)) + // { + // robot::printf_yellow("[%s:%d] Loading default recovery behaviors\n", __FILE__, __LINE__); + // loadDefaultRecoveryBehaviors(); + // } - // initially, we'll need to make a plan - state_ = move_base::PLANNING; + // // initially, we'll need to make a plan + // state_ = move_base::PLANNING; - // we'll start executing recovery behaviors at the beginning of our list - recovery_index_ = 0; - nav_feedback_ = std::make_shared(); - if (nav_feedback_) - { - nav_feedback_->navigation_state = move_base_core::State::PLANNING; - nav_feedback_->is_ready = true; - } - else - { - robot::printf_red("[%s:%d] ERROR: nav_feedback_ is nullptr\n", __FILE__, __LINE__); - } + // // we'll start executing recovery behaviors at the beginning of our list + // recovery_index_ = 0; + // nav_feedback_ = std::make_shared(); + // if (nav_feedback_) + // { + // nav_feedback_->navigation_state = move_base_core::State::PLANNING; + // nav_feedback_->is_ready = true; + // } + // else + // { + // robot::printf_red("[%s:%d] ERROR: nav_feedback_ is nullptr\n", __FILE__, __LINE__); + // } - initialized_ = true; - printf("[%s:%d] ========== End: initialize() - SUCCESS ==========\n", __FILE__, __LINE__); + // initialized_ = true; + // printf("[%s:%d] ========== End: initialize() - SUCCESS ==========\n", __FILE__, __LINE__); } else {