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
This commit is contained in:
parent
a7c4b39c6e
commit
a06beb70b8
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -1,3 +0,0 @@
|
|||
base_global_planner: TwoPointsPlanner
|
||||
TwoPointsPlanner:
|
||||
lethal_obstacle: 20
|
||||
|
|
@ -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;
|
||||
|
||||
// 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());
|
||||
robot::NodeHandle nh1 = robot::NodeHandle(nh);
|
||||
nh1.setParam("local_costmap/obstacles/enabled", false);
|
||||
// nh.printAllParams();
|
||||
// nh1.printAllParams();
|
||||
// nh.printAllParams();
|
||||
|
||||
// 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;
|
||||
// // 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());
|
||||
|
||||
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;
|
||||
// // 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;
|
||||
|
||||
// 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"));
|
||||
// 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;
|
||||
|
||||
// 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<geometry_msgs::PoseStamped>();
|
||||
latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
|
||||
controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
|
||||
// // 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"));
|
||||
|
||||
// set up the planner's thread
|
||||
planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this));
|
||||
// // 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<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
|
||||
// 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);
|
||||
// // set up the planner's thread
|
||||
// planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this));
|
||||
|
||||
// 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<nav_core::BaseGlobalPlanner::Ptr()>(
|
||||
"/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);
|
||||
// // 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);
|
||||
|
||||
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<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");
|
||||
}
|
||||
// // 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<nav_core::BaseGlobalPlanner::Ptr()>(
|
||||
// "/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);
|
||||
|
||||
// Start actively updating costmaps based on sensor data
|
||||
planner_costmap_robot_->start();
|
||||
controller_costmap_robot_->start();
|
||||
// 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<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
|
||||
{
|
||||
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<move_base_core::NavFeedback>();
|
||||
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<move_base_core::NavFeedback>();
|
||||
// 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
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user