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

update

Browse Source
This commit is contained in:
HiepLM
2025-12-16 16:18:35 +07:00
parent a06beb70b8
commit 89f435633c
47 changed files with 600 additions and 881 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/Navigations/Cores/nav_core2/CMakeLists.txt
View File

@@ -28,6 +28,8 @@ target_link_libraries(nav_core2 INTERFACE
tf3
nav_grid
nav_2d_msgs
robot::node_handle
robot::console
)
# Set include directories

4
src/Navigations/Cores/nav_core2/include/nav_core2/costmap.h
View File

@@ -35,13 +35,13 @@
#ifndef NAV_CORE2_COSTMAP_H
#define NAV_CORE2_COSTMAP_H
#include <robot/node_handle.h>
#include <nav_grid/nav_grid.h>
#include <nav_core2/common.h>
#include <nav_core2/bounds.h>
#include <boost/thread.hpp>
#include <memory>
#include <string>
#include <yaml-cpp/yaml.h>
namespace nav_core2
{
@@ -71,7 +71,7 @@ public:
* @param name The namespace for the costmap
* @param tf A pointer to a transform listener
*/
virtual void initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf) {}
virtual void initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf) {}
inline unsigned char getCost(const unsigned int x, const unsigned int y)
{

2
src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h
View File

@@ -68,7 +68,7 @@ public:
* @param tf A pointer to a transform listener
* @param costmap A pointer to the costmap
*/
virtual void initialize(const YAML::Node& parent, const std::string& name,
virtual void initialize(const robot::NodeHandle& parent, const std::string& name,
TFListenerPtr tf, Costmap::Ptr costmap) = 0;
/**

2
src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h
View File

@@ -76,7 +76,7 @@ namespace nav_core2
// virtual void initialize(YAML::Node &parent, const std::string &name,
// TFListenerPtr tf, Costmap::Ptr costmap) = 0;
virtual void initialize(YAML::Node &parent, const std::string &name,
virtual void initialize(robot::NodeHandle &parent, const std::string &name,
TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap) = 0;
/**

4
src/Navigations/Cores/nav_core_adapter/CMakeLists.txt
View File

@@ -75,7 +75,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON)
option(BUILD_TESTS "Build test programs" OFF)
# Flags biên dịch
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
# Warning flags - disabled to suppress warnings during build
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings
set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")

2
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h
View File

@@ -60,7 +60,7 @@ public:
void initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction = false);
// Standard Costmap Interface
void initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf) override;
void initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf) override;
nav_core2::Costmap::mutex_t* getMutex() override;
// NavGrid Interface

2
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h
View File

@@ -55,7 +55,7 @@ public:
GlobalPlannerAdapter();
// Nav Core 2 Global Planner Interface
void initialize(const YAML::Node& parent, const std::string& name,
void initialize(const robot::NodeHandle& parent, const std::string& name,
TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start,
const nav_2d_msgs::Pose2DStamped& goal) override;

5
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h
View File

@@ -35,6 +35,7 @@
#ifndef NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H
#define NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H
#include <robot/node_handle.h>
#include <nav_core/base_local_planner.h>
#include <nav_core2/local_planner.h>
#include <nav_core_adapter/costmap_adapter.h>
@@ -197,8 +198,8 @@ namespace nav_core_adapter
boost::recursive_mutex configuration_mutex_;
// std::shared_ptr<dynamic_reconfigure::Server<nav_core_adapter::NavCoreAdapterConfig>> dsrv_;
YAML::Node private_nh_;
YAML::Node adapter_nh_;
robot::NodeHandle private_nh_;
robot::NodeHandle adapter_nh_;
// void reconfigureCB(nav_core_adapter::NavCoreAdapterConfig &config, uint32_t level);
nav_core_adapter::NavCoreAdapterConfig last_config_;
nav_core_adapter::NavCoreAdapterConfig default_config_;

4
src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp
View File

@@ -72,10 +72,10 @@ void CostmapAdapter::initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool
costmap_ = costmap_robot_->getCostmap();
}
void CostmapAdapter::initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf)
void CostmapAdapter::initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf)
{
// TODO: Implement this if needed
throw nav_core2::CostmapException("initialize with YAML::Node not implemented");
throw nav_core2::CostmapException("initialize with robot::NodeHandle not implemented");
}
nav_core2::Costmap::mutex_t* CostmapAdapter::getMutex()

2
src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp
View File

@@ -53,7 +53,7 @@ namespace nav_core_adapter
/**
* @brief Load the nav_core global planner and initialize it
*/
void GlobalPlannerAdapter::initialize(const YAML::Node& parent, const std::string& name,
void GlobalPlannerAdapter::initialize(const robot::NodeHandle& parent, const std::string& name,
TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
{
costmap_ = costmap;

37
src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp
View File

@@ -32,6 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <robot/console.h>
#include <nav_core_adapter/local_planner_adapter.h>
#include <nav_core_adapter/costmap_adapter.h>
#include <nav_core_adapter/shared_pointers.h>
@@ -79,23 +80,22 @@ namespace nav_core_adapter
costmap_adapter_ = std::make_shared<CostmapAdapter>();
costmap_adapter_->initialize(costmap_robot);
YAML::Node nh;
private_nh_ = YAML::Node("~");
adapter_nh_ = YAML::Node("~/" + name);
std::cout << "Adapter namespace: " << adapter_nh_["~"].as<std::string>() << std::endl;
robot::NodeHandle nh;
private_nh_ = robot::NodeHandle("~");
adapter_nh_ = robot::NodeHandle(private_nh_, name);
robot::log_info("Adapter namespace: %s", private_nh_.getNamespace().c_str());
std::string planner_name;
if (adapter_nh_["planner_name"] && adapter_nh_["planner_name"].IsDefined())
if (adapter_nh_.hasParam("planner_name"))
{
planner_name = adapter_nh_["planner_name"].as<std::string>();
adapter_nh_.getParam("planner_name", planner_name, std::string("dwb_local_planner::DWBLocalPlanner"));
}
else
{
planner_name = nav_2d_utils::loadParameterWithDeprecation(
adapter_nh_, "planner_name", "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
adapter_nh_["planner_name"] = planner_name;
adapter_nh_.setParam("planner_name", planner_name);
}
std::cout << "Loading plugin " << planner_name << std::endl;
robot::log_info("Loading plugin %s", planner_name.c_str());
// planner_ = planner_loader_.createInstance(planner_name);
std::string path_file_so;
@@ -104,10 +104,10 @@ namespace nav_core_adapter
planner_ = planner_loader_();
if (!planner_)
{
std::cerr << "Failed to load planner " << planner_name << std::endl;
robot::log_error("Failed to load planner %s", planner_name.c_str());
exit(EXIT_FAILURE);
}
planner_->initialize(private_nh_, adapter_nh_["planner_name"].as<std::string>(), tf_, costmap_robot_);
planner_->initialize(private_nh_, planner_name, tf_, costmap_robot_);
// odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(nh);
// dsrv_ = std::make_shared<dynamic_reconfigure::Server<nav_core_adapter::NavCoreAdapterConfig>>(configuration_mutex_, adapter_nh_);
@@ -140,26 +140,17 @@ namespace nav_core_adapter
std::cerr << "Failed to load planner " << planner_name << std::endl;
exit(EXIT_FAILURE);
}
new_planner->initialize(private_nh_, adapter_nh_["planner_name"].as<std::string>(),
tf_, costmap_robot_);
if (!new_planner)
{
std::cerr << "Failed to load planner " << planner_name << std::endl;
exit(EXIT_FAILURE);
}
new_planner->initialize(private_nh_, adapter_nh_["planner_name"].as<std::string>(),
tf_, costmap_robot_);
new_planner->initialize(private_nh_, planner_name, tf_, costmap_robot_);
if (planner_)
planner_.reset();
planner_ = new_planner;
last_config_.planner_name = planner_name;
std::cout << "Loaded new planner: " << planner_name << std::endl;
robot::log_info("Loaded new planner: %s", planner_name.c_str());
}
catch (const std::exception &ex)
{
std::cerr << "Failed to load planner " << planner_name << ": " << ex.what() << std::endl;
robot::log_error("Failed to load planner %s: %s", planner_name.c_str(), ex.what());
return false;
}
}

4
src/Navigations/Packages/move_base/CMakeLists.txt
View File

@@ -237,7 +237,9 @@ option(BUILD_TESTS "Build test programs" OFF)
# ========================================================
# Compiler Flags
# ========================================================
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
# Warning flags - disabled to suppress warnings during build
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings
set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")

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

@@ -73,201 +73,196 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
// NodeHandle("~") will automatically load YAML files from config directory
robot::NodeHandle nh("~");
private_nh_ = nh;
private_nh_.printAllParams();
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
// 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<geometry_msgs::PoseStamped>();
// latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
// controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
// 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>();
// // 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<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);
// 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);
// 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");
// }
planner_ = planner_loader_();
if (!planner_)
{
robot::log_error("[%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_))
robot::log_info("[%s:%d] Global planner initialized successfully", __FILE__, __LINE__);
else
robot::log_error("[%s:%d] Global planner initialized failed", __FILE__, __LINE__);
}
catch (const std::exception &ex)
{
robot::log_error("[%s:%d] EXCEPTION in global planner: %s", __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::log_error("[%s:%d] ERROR: controller_loader_() returned nullptr", __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)
{
robot::log_error("[%s:%d] EXCEPTION in local planner: %s", __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::log_warning("[%s:%d] Loading default recovery behaviors", __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::log_error("[%s:%d] ERROR: nav_feedback_ is nullptr", __FILE__, __LINE__);
}
// initialized_ = true;
// printf("[%s:%d] ========== End: initialize() - SUCCESS ==========\n", __FILE__, __LINE__);
initialized_ = true;
robot::log_info("[%s:%d] ========== End: initialize() - SUCCESS ==========", __FILE__, __LINE__);
}
else
{
robot::printf_yellow("[%s:%d] Already initialized, skipping\n", __FILE__, __LINE__);
robot::log_warning("[%s:%d] Already initialized, skipping", __FILE__, __LINE__);
}
}