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

Compare commits

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

No commits in common. "a06beb70b875c1f558b668d9d6a60dc582cf458b" and "2b7cee7642ccc413fd6bfa88036e6cbbf020be8a" have entirely different histories.
a06beb70b8 ... 2b7cee7642

4 changed files with 174 additions and 176 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 Normal file
View File

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

2
src/Libraries/robot_cpp

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

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

@ -32,6 +32,7 @@ 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()
@ -69,201 +70,195 @@ 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;
robot::NodeHandle nh1 = robot::NodeHandle(nh); // get some parameters that will be global to the move base node
nh1.setParam("local_costmap/obstacles/enabled", false); printf("[%s:%d] Getting planner parameters\n", __FILE__, __LINE__);
// nh.printAllParams(); std::string global_planner, local_planner;
// nh1.printAllParams(); private_nh_.getParam("base_global_planner", global_planner, std::string(""));
// nh.printAllParams(); 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 // Handle nested YAML nodes for costmap config
// std::string global_planner, local_planner; std::string robot_base_frame;
// private_nh_.getParam("base_global_planner", global_planner, std::string("")); private_nh_.getParam("robot_base_frame", robot_base_frame, std::string("base_footprint"));
// printf("[%s:%d] global_planner: %s\n", __FILE__, __LINE__, global_planner.c_str()); std::string global_frame;
// private_nh_.getParam("base_local_planner", local_planner, std::string("")); private_nh_.getParam("global_frame", global_frame, std::string("map"));
// printf("[%s:%d] local_planner: %s\n", __FILE__, __LINE__, local_planner.c_str()); 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 double original_xy_goal_tolerance;
// std::string robot_base_frame; private_nh_.getParam("xy_goal_tolerance", original_xy_goal_tolerance, 0.2);
// private_nh_.getParam("robot_base_frame", robot_base_frame, std::string("base_footprint")); original_xy_goal_tolerance_ = original_xy_goal_tolerance;
// std::string global_frame; double original_yaw_goal_tolerance;
// private_nh_.getParam("global_frame", global_frame, std::string("map")); private_nh_.getParam("yaw_goal_tolerance", original_yaw_goal_tolerance, 0.2);
// robot_base_frame_ = robot_base_frame; original_yaw_goal_tolerance_ = original_yaw_goal_tolerance;
// 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; // defined local planner name
// private_nh_.getParam("xy_goal_tolerance", original_xy_goal_tolerance, 0.2); private_nh_.getParam("position_planner_name", position_planner_name_, std::string("mkt_algorithm/MKTPositionPlanner"));
// original_xy_goal_tolerance_ = original_xy_goal_tolerance; private_nh_.getParam("docking_planner_name", docking_planner_name_, std::string("mkt_algorithm/MKTDockingPlanner"));
// double original_yaw_goal_tolerance; private_nh_.getParam("go_straight_planner_name", go_straight_planner_name_, std::string("mkt_algorithm/MKTGoStraightPlanner"));
// private_nh_.getParam("yaw_goal_tolerance", original_yaw_goal_tolerance, 0.2); private_nh_.getParam("rotate_planner_name", rotate_planner_name_, std::string("mkt_algorithm/MKTRotatePlanner"));
// original_yaw_goal_tolerance_ = original_yaw_goal_tolerance;
// // defined local planner name // parameters of make_plan service
// private_nh_.getParam("position_planner_name", position_planner_name_, std::string("mkt_algorithm/MKTPositionPlanner")); private_nh_.getParam("make_plan_clear_costmap", make_plan_clear_costmap_, false);
// private_nh_.getParam("docking_planner_name", docking_planner_name_, std::string("mkt_algorithm/MKTDockingPlanner")); private_nh_.getParam("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, false);
// private_nh_.getParam("go_straight_planner_name", go_straight_planner_name_, std::string("mkt_algorithm/MKTGoStraightPlanner")); private_nh_.getParam("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
// private_nh_.getParam("rotate_planner_name", rotate_planner_name_, std::string("mkt_algorithm/MKTRotatePlanner")); 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>();
// // parameters of make_plan service // set up the planner's thread
// private_nh_.getParam("make_plan_clear_costmap", make_plan_clear_costmap_, false); planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this));
// 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>();
// // set up the planner's thread // we'll assume the radius of the robot to be consistent with what's specified for the costmaps
// planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this)); // 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 // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
// // From config param planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
// double inscribed_radius; planner_costmap_robot_->pause();
// private_nh_.getParam("local_costmap/inscribed_radius", inscribed_radius, 0.2); // initialize the global planner
// double circumscribed_radius; try
// private_nh_.getParam("local_costmap/circumscribed_radius", circumscribed_radius, 0.3); {
// inscribed_radius_ = inscribed_radius; planner_loader_ = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>(
// circumscribed_radius_ = circumscribed_radius; "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/global_planners/two_points_planner/libtwo_points_planner.so",
// private_nh_.getParam("clearing_radius", clearing_radius_, 0.0); global_planner,
// double conservative_reset_dist; boost::dll::load_mode::append_decorations);
// 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_ = planner_loader_();
// planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_); if (!planner_)
// planner_costmap_robot_->pause(); {
// // initialize the global planner robot::printf_red("[%s:%d] ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__);
// try throw std::runtime_error("Failed to load global planner " + global_planner);
// { }
// planner_loader_ = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>( if(planner_->initialize(global_planner, planner_costmap_robot_))
// "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/global_planners/two_points_planner/libtwo_points_planner.so", printf("[%s:%d] Global planner initialized successfully\n", __FILE__, __LINE__);
// global_planner, else
// boost::dll::load_mode::append_decorations); 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");
}
// planner_ = planner_loader_(); // Start actively updating costmaps based on sensor data
// if (!planner_) planner_costmap_robot_->start();
// { 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
{ {