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:
|
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
|
||||||
|
|
|
||||||
|
|
@ -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),
|
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
|
||||||
{
|
{
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue
Block a user