diff --git a/config/costmap_global_params_plugins_no_virtual_walls.yaml b/config/costmap_global_params_plugins_no_virtual_walls.yaml index d79c6a7..ce17e0f 100755 --- a/config/costmap_global_params_plugins_no_virtual_walls.yaml +++ b/config/costmap_global_params_plugins_no_virtual_walls.yaml @@ -2,7 +2,7 @@ global_costmap: frame_id: map plugins: - {name: navigation_map, type: "StaticLayer" } - - {name: virtual_walls_map, type: "StaticLayer" } + # - {name: virtual_walls_map, type: "StaticLayer" } - {name: obstacles, type: "VoxelLayer" } - {name: inflation, type: "InflationLayer" } obstacles: diff --git a/config/custom_global_params.yaml b/config/custom_global_params.yaml index f4eaf44..71565e3 100644 --- a/config/custom_global_params.yaml +++ b/config/custom_global_params.yaml @@ -10,6 +10,8 @@ CustomPlanner: nominalvel_mpersecs: 0.8 timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s allow_unknown: true - + directory_to_save_paths: "/init/paths" + pathway_filename: "pathway.txt" + current_pose_topic_name: "/amcl_pose" diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index 547ecd0..40dc56d 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -5,45 +5,49 @@ go_straight_planner_name: PNKXGoStraightLocalPlanner rotate_planner_name: PNKXRotateLocalPlanner base_local_planner: LocalPlannerAdapter -yaw_goal_tolerance: 0.017 -xy_goal_tolerance: 0.03 -min_approach_linear_velocity: 0.06 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05) -stateful: true -publish_topic: true - LocalPlannerAdapter: - PNKXLocalPlanner: - # Algorithm - trajectory_generator_name: LimitedAccelGenerator - algorithm_nav_name: MKTAlgorithmDiffPredictiveTrajectory - algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal - # Goal checking - goal_checker_name: GoalChecker + library_path: liblocal_planner_adapter + yaw_goal_tolerance: 0.017 + xy_goal_tolerance: 0.03 + min_approach_linear_velocity: 0.06 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05) - PNKXDockingLocalPlanner: - # Algorithm - trajectory_generator_name: LimitedAccelGenerator - algorithm_nav_name: MKTAlgorithmDiffPredictiveTrajectory - algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal - # Goal checking - goal_checker_name: GoalChecker +PNKXLocalPlanner: + # Algorithm + library_path: libpnkx_local_planner + trajectory_generator_name: LimitedAccelGenerator + algorithm_nav_name: MKTAlgorithmDiffPredictiveTrajectory + algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal + # Goal checking + goal_checker_name: GoalChecker - PNKXGoStraightLocalPlanner: - # Algorithm - trajectory_generator_name: LimitedAccelGenerator - algorithm_nav_name: MKTAlgorithmDiffGoStraight - # Goal checking - goal_checker_name: GoalChecker +PNKXDockingLocalPlanner: + # Algorithm + library_path: libpnkx_local_planner + trajectory_generator_name: LimitedAccelGenerator + algorithm_nav_name: MKTAlgorithmDiffPredictiveTrajectory + algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal + # Goal checking + goal_checker_name: GoalChecker - PNKXRotateLocalPlanner: - # Algorithm - algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal - trajectory_generator_name: LimitedAccelGenerator - # Goal checking - goal_checker_name: SimpleGoalChecker - stateful: true +PNKXGoStraightLocalPlanner: + # Algorithm + library_path: libpnkx_local_planner + trajectory_generator_name: LimitedAccelGenerator + algorithm_nav_name: MKTAlgorithmDiffGoStraight + # Goal checking + goal_checker_name: GoalChecker + +PNKXRotateLocalPlanner: + # Algorithm + library_path: libpnkx_local_planner + algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal + trajectory_generator_name: LimitedAccelGenerator + # Goal checking + goal_checker_name: SimpleGoalChecker + stateful: true LimitedAccelGenerator: + library_path: libmkt_plugins_standard_traj_generator max_vel_x: 1.2 min_vel_x: -1.2 @@ -75,7 +79,8 @@ LimitedAccelGenerator: # sim_period include_last_point: true -PredictiveTrajectory: +MKTAlgorithmDiffPredictiveTrajectory: + library_path: libmkt_algorithm_diff avoid_obstacles: false xy_local_goal_tolerance: 0.01 angle_threshold: 0.6 @@ -115,7 +120,8 @@ PredictiveTrajectory: cost_scaling_dist: 0.2 # (default: 0.6) cost_scaling_gain: 2.0 # (default: 1.0) -GoStraight: +MKTAlgorithmDiffGoStraight: + library_path: libmkt_algorithm_diff avoid_obstacles: false xy_local_goal_tolerance: 0.01 angle_threshold: 0.6 @@ -145,7 +151,8 @@ GoStraight: use_regulated_linear_velocity_scaling: false use_cost_regulated_linear_velocity_scaling: false -RotateToGoal: +MKTAlgorithmDiffRotateToGoal: + library_path: libmkt_algorithm_diff avoid_obstacles: false xy_local_goal_tolerance: 0.01 angle_threshold: 0.6 @@ -183,4 +190,11 @@ RotateToGoal: use_cost_regulated_linear_velocity_scaling: false # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true) inflation_cost_scaling_factor: 2.0 # (default: 3.0) # must be > 0 cost_scaling_dist: 0.2 # (default: 0.6) - cost_scaling_gain: 2.0 # (default: 1.0) \ No newline at end of file + cost_scaling_gain: 2.0 # (default: 1.0) + + +GoalChecker: + library_path: libmkt_plugins_goal_checker + +SimpleGoalChecker: + library_path: libmkt_plugins_simple_goal_checker \ No newline at end of file diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index d7c12b8..873f628 100755 Binary files a/examples/NavigationExample/libnav_c_api.so and b/examples/NavigationExample/libnav_c_api.so differ diff --git a/examples/run_example.sh b/examples/run_example.sh index 9a04a48..705171a 100755 --- a/examples/run_example.sh +++ b/examples/run_example.sh @@ -90,14 +90,21 @@ echo "Copying library..." cp "$LIB_DIR/libnav_c_api.so" . # Bước 4: Set LD_LIBRARY_PATH để tìm được tất cả dependencies -export LD_LIBRARY_PATH="$LIB_DIR:$LD_LIBRARY_PATH" -export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH" -export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH" -export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH" +# Main build directory (contains libtf3.so, librobot_cpp.so, etc.) +export LD_LIBRARY_PATH="$BUILD_DIR:$LD_LIBRARY_PATH" +# Library directories for dependencies +export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/robot_time:$LD_LIBRARY_PATH" +export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/xmlrpcpp:$LD_LIBRARY_PATH" +export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/costmap_2d:$LD_LIBRARY_PATH" +export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH" +export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH" export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH" export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH" export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH" -export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/robot_time:$LD_LIBRARY_PATH" +# Also add the example directory where the library is copied +export LD_LIBRARY_PATH="$EXAMPLE_DIR/NavigationExample:$LD_LIBRARY_PATH" +# Add the output directory where .NET places the library +export LD_LIBRARY_PATH="$EXAMPLE_DIR/NavigationExample/bin/Debug/net6.0/linux-x64:$LD_LIBRARY_PATH" # Bước 5: Build và chạy echo "Building C# project..." diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp index 8c129b6..d131227 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp @@ -5,10 +5,6 @@ #include #include #include - -// #include -// #include -// #include #include #include @@ -18,6 +14,7 @@ #include #include #include +#include pnkx_local_planner::PNKXLocalPlanner::PNKXLocalPlanner() : initialized_(false) @@ -61,7 +58,6 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, parent_ = parent; planner_nh_ = robot::NodeHandle(parent_, name); - // planner_nh_.printRootParams(); this->getParams(planner_nh_); std::string traj_generator_name; @@ -69,7 +65,8 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str()); try { - std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Libraries/mkt_plugins/libmkt_plugins_standard_traj_generator.so"; + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(traj_generator_name); traj_gen_loader_ = boost::dll::import_alias( path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations); traj_generator_ = traj_gen_loader_(); @@ -93,7 +90,8 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str()); try { - std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Libraries/mkt_algorithm/libmkt_algorithm_diff.so"; + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(algorithm_nav_name); nav_algorithm_loader_ = boost::dll::import_alias( path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations); nav_algorithm_ = nav_algorithm_loader_(); @@ -114,7 +112,8 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("MKTAlgorithmDiffRotateToGoal")); try { - std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Libraries/mkt_algorithm/libmkt_algorithm_diff.so"; + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(algorithm_rotate_name); rotate_algorithm_loader_ = boost::dll::import_alias( path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations); rotate_algorithm_ = rotate_algorithm_loader_(); @@ -136,7 +135,8 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker")); try { - std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Libraries/mkt_plugins/libmkt_plugins_goal_checker.so"; + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(goal_checker_name); goal_checker_loader_ = boost::dll::import_alias( path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations); goal_checker_ = goal_checker_loader_(); diff --git a/src/Libraries/costmap_2d b/src/Libraries/costmap_2d index 10521c1..0344c31 160000 --- a/src/Libraries/costmap_2d +++ b/src/Libraries/costmap_2d @@ -1 +1 @@ -Subproject commit 10521c1629301d3e81f040213f08de7881bc5e59 +Subproject commit 0344c31e5b4185da41bfb253db720d9c91b802bd diff --git a/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp b/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp index 47a166c..43ef0ba 100755 --- a/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp +++ b/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp @@ -95,7 +95,8 @@ namespace nav_core_adapter } try { - std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(planner_name); planner_loader_ = boost::dll::import_alias( path_file_so, planner_name, boost::dll::load_mode::append_decorations); planner_ = planner_loader_(); @@ -105,7 +106,7 @@ namespace nav_core_adapter exit(EXIT_FAILURE); } robot::log_info("\nLocalPlannerAdapter initialize: %s", planner_name.c_str()); - planner_->initialize(adapter_nh_, planner_name, tf_, costmap_robot_); + planner_->initialize(private_nh_, planner_name, tf_, costmap_robot_); robot::log_info("[%s:%d]\n Successfully initialized planner \"%s\"", __FILE__, __LINE__, planner_name.c_str()); } catch (const boost::system::system_error &ex) diff --git a/src/Navigations/Packages/move_base/CMakeLists.txt b/src/Navigations/Packages/move_base/CMakeLists.txt index 4b73e49..2b24b31 100644 --- a/src/Navigations/Packages/move_base/CMakeLists.txt +++ b/src/Navigations/Packages/move_base/CMakeLists.txt @@ -117,6 +117,7 @@ if(BUILDING_WITH_CATKIN) move_base_core nav_core costmap_2d + plugins # Link với plugins library để có StaticLayer typeinfo xmlrpcpp robot_cpp tf3_sensor_msgs @@ -139,6 +140,7 @@ else() move_base_core nav_core costmap_2d + plugins # Link với plugins library để có StaticLayer typeinfo yaml-cpp xmlrpcpp tf3_sensor_msgs diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index dbf4217..6bf0dea 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -17,6 +17,11 @@ #include #include +#include +#include +#include +#include + move_base::MoveBase::MoveBase() : initialized_(false), planner_costmap_robot_(NULL), controller_costmap_robot_(NULL), @@ -175,6 +180,35 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) { planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_); planner_costmap_robot_->pause(); + costmap_2d::LayeredCostmap* layered_costmap_ = planner_costmap_robot_->getLayeredCostmap(); + for (std::vector>::const_iterator layer = layered_costmap_->getPlugins()->begin(); + layer != layered_costmap_->getPlugins()->end(); + ++layer) + { + boost::shared_ptr static_layer = boost::dynamic_pointer_cast(*layer); + if (!static_layer) + continue; + nav_msgs::OccupancyGrid *occupancy_grid = new nav_msgs::OccupancyGrid(); + occupancy_grid->header.frame_id = "map"; + occupancy_grid->header.stamp = robot::Time::now(); + occupancy_grid->info.resolution = 0.05; + occupancy_grid->info.width = 100; + occupancy_grid->info.height = 100; + occupancy_grid->info.origin.position.x = 0.0; + occupancy_grid->info.origin.position.y = 0.0; + occupancy_grid->info.origin.position.z = 0.0; + occupancy_grid->info.origin.orientation.x = 0.0; + occupancy_grid->info.origin.orientation.y = 0.0; + occupancy_grid->info.origin.orientation.z = 0.0; + occupancy_grid->info.origin.orientation.w = 1.0; + occupancy_grid->data.resize(occupancy_grid->info.width * occupancy_grid->info.height); + for (int i = 0; i < occupancy_grid->data.size(); i++) + { + occupancy_grid->data[i] = costmap_2d::NO_INFORMATION; + } + if (occupancy_grid) + static_layer->dataCallBack(*occupancy_grid, "map"); + } } catch (const std::exception &ex) { @@ -210,6 +244,32 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) { controller_costmap_robot_ = new costmap_2d::Costmap2DROBOT("local_costmap", *tf_); controller_costmap_robot_->pause(); + costmap_2d::LayeredCostmap* layered_costmap_ = controller_costmap_robot_->getLayeredCostmap(); + for (std::vector>::const_iterator layer = layered_costmap_->getPlugins()->begin(); + layer != layered_costmap_->getPlugins()->end(); + ++layer) + { + boost::shared_ptr obstacle_layer = boost::dynamic_pointer_cast(*layer); + if (!obstacle_layer) + continue; + sensor_msgs::LaserScan *laser_scan = new sensor_msgs::LaserScan(); + laser_scan->header.frame_id = "laser"; + laser_scan->header.stamp = robot::Time::now(); + laser_scan->angle_min = -M_PI; + laser_scan->angle_max = M_PI; + laser_scan->angle_increment = M_PI / 180; + laser_scan->time_increment = 0.0; + laser_scan->scan_time = 0.0; + laser_scan->range_min = 0.0; + laser_scan->range_max = 10.0; + laser_scan->ranges.resize(180); + for (int i = 0; i < 180; i++) + { + laser_scan->ranges[i] = 10.0; + } + if (laser_scan) + obstacle_layer->dataCallBack(*laser_scan, "laser"); + } } catch (const std::exception &ex) { diff --git a/src/Navigations/Packages/move_base/src/move_base_main.cpp b/src/Navigations/Packages/move_base/src/move_base_main.cpp index 597e051..3c3dfdf 100644 --- a/src/Navigations/Packages/move_base/src/move_base_main.cpp +++ b/src/Navigations/Packages/move_base/src/move_base_main.cpp @@ -33,15 +33,65 @@ #include #include #include +#include +#include int main(int argc, char** argv) { try { std::shared_ptr tf = std::make_shared(); + // Lambda function để set static transform (tương tự tf_listener_set_static_transform trong nav_c_api.cpp) + auto set_static_transform = [&tf](const std::string& parent_frame, + const std::string& child_frame, + double x, double y, double z, + double qx, double qy, double qz, double qw) -> bool { + if (!tf || parent_frame.empty() || child_frame.empty()) { + return false; + } + + try { + tf3::TransformStampedMsg st; + st.header.stamp = tf3::Time::now(); + st.header.frame_id = parent_frame; + st.child_frame_id = child_frame; + st.transform.translation.x = x; + st.transform.translation.y = y; + st.transform.translation.z = z; + st.transform.rotation.x = qx; + st.transform.rotation.y = qy; + st.transform.rotation.z = qz; + st.transform.rotation.w = qw; + + return tf->setTransform(st, "move_base_main(static)", true /*is_static*/); + } catch (...) { + return false; + } + }; + + if (set_static_transform("map", "odom", 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)) { + robot::log_info("[%s:%d]\n Static transform set: map -> odom", __FILE__, __LINE__); + } else { + robot::log_error("[%s:%d]\n Failed to set static transform: map -> odom", __FILE__, __LINE__); + } + + if (set_static_transform("odom", "base_footprint", 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)) { + robot::log_info("[%s:%d]\n Static transform set: odom -> base_footprint", __FILE__, __LINE__); + } else { + robot::log_error("[%s:%d]\n Failed to set static transform: odom -> base_footprint", __FILE__, __LINE__); + } + + if (set_static_transform("base_footprint", "base_link", 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)) { + robot::log_info("[%s:%d]\n Static transform set: base_footprint -> base_link", __FILE__, __LINE__); + } else { + robot::log_error("[%s:%d]\n Failed to set static transform: base_footprint -> base_link", __FILE__, __LINE__); + } + robot::log_info("[%s:%d]\n Loading MoveBase library...", __FILE__, __LINE__); + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath("MoveBase"); auto create_plugin = boost::dll::import_alias( - "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Packages/move_base/libmove_base.so", + path_file_so, "MoveBase", boost::dll::load_mode::append_decorations); robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__); @@ -57,7 +107,6 @@ int main(int argc, char** argv) robot::log_info("[%s:%d]\n Navigation initialized successfully", __FILE__, __LINE__); std::cout << "Move Base Main Function :" << robot::Time::now().toSec() << std::endl; - // Explicitly reset before create_plugin goes out of scope to ensure proper cleanup order robot::log_info("[%s:%d]\n Cleaning up...", __FILE__, __LINE__); nav_ptr.reset(); @@ -71,29 +120,5 @@ int main(int argc, char** argv) robot::log_error("[%s:%d]\n Unknown exception occurred", __FILE__, __LINE__); return 1; } - - - // robot::NodeHandle private_nh_ = robot::NodeHandle("~"); - // robot::NodeHandle adapter_nh_ = robot::NodeHandle(private_nh_, "LocalPlannerAdapter"); - // robot::log_info_at(__FILE__, __LINE__, "\nUsing adapter namespace: %s", adapter_nh_.getNamespace().c_str()); - - - // std::string planner_name; - // if (adapter_nh_.hasParam("planner_name")) - // { - // adapter_nh_.getParam("planner_name", planner_name, std::string("dwb_local_planner::DWBLocalPlanner")); - // } - // else - // { - // planner_name = nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner")); - // adapter_nh_.setParam("planner_name", planner_name); - // } - // robot::log_info_at(__FILE__, __LINE__, "Planner name: %s", planner_name.c_str()); - // robot::NodeHandle parent_; - // parent_ = adapter_nh_; - // robot::NodeHandle planner_nh_ = robot::NodeHandle(parent_, "PNKXLocalPlanner"); - // std::string algorithm_nav_name; - // planner_nh_.getParam("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); - // robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str()); return 0; } \ No newline at end of file