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
2026-03-20 16:06:47 +07:00
parent 9a4bb95c4c
commit d38f6b3954
8 changed files with 34 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
config/dock_global_params.yaml
View File

@@ -1,6 +1,5 @@
DockPlanner:
library_path: libdock_planner
# File: config/planner_params.yaml
MyGlobalPlanner:
cost_threshold: 200 # Ngưỡng chi phí vật cản (0-255)
safety_distance: 2 # Khoảng cách an toàn (cells)

2
examples/NavigationExample/NavigationExample.csproj
View File

@@ -1,7 +1,7 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<OutputType>Exe</OutputType>
<TargetFramework>net10.0</TargetFramework>
<TargetFramework>net6.0</TargetFramework>
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
</PropertyGroup>
<ItemGroup>

4
examples/NavigationExample/Program.cs
View File

@@ -396,8 +396,8 @@ namespace NavigationExample
PoseStamped goal = new PoseStamped();
goal.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId));
goal.pose.position.x = 1.0;
goal.pose.position.y = 1.0;
goal.pose.position.x = 0.01;
goal.pose.position.y = 0.01;
goal.pose.position.z = 0.0;
goal.pose.orientation.x = 0.0;
goal.pose.orientation.y = 0.0;

2
src/Algorithms/Packages/global_planners/dock_planner

Submodule src/Algorithms/Packages/global_planners/dock_planner updated: da82431cd9...56d05e4322

20
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp
View File

@@ -74,7 +74,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
if (!traj_generator_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
}
robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
traj_generator_->initialize(nh_traj_gen);
@@ -83,7 +83,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
catch (const std::exception &ex)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
}
std::string algorithm_nav_name;
@@ -99,14 +99,14 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
if (!nav_algorithm_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
}
nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
}
catch (const std::exception &ex)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
}
std::string algorithm_rotate_name;
@@ -121,7 +121,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
if (!rotate_algorithm_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
}
rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());
@@ -129,7 +129,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
catch (const std::exception &ex)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
}
std::string goal_checker_name;
@@ -145,7 +145,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
if (!goal_checker_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
}
goal_checker_->initialize(parent_);
robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
@@ -153,7 +153,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
catch (const std::exception &ex)
{
robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
}
this->initializeOthers();
@@ -601,7 +601,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
if (!docking_planner_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create global planner \"%s\": returned null pointer", docking_planner_name_.c_str());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create global planner");
}
docking_planner_->initialize(name_, costmap_robot_);
robot::log_info_at(__FILE__, __LINE__, "Created global_planner %s", docking_planner_name_.c_str());
@@ -625,7 +625,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
if (!docking_nav_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create docking nav \"%s\": returned null pointer", docking_nav_name_.c_str());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create docking nav");
}
robot::NodeHandle nh_docking_nav = robot::NodeHandle(nh_priv_, docking_nav_name_);
docking_nav_->initialize(nh_docking_nav, docking_nav_name_, tf_, costmap_robot_, traj_generator_);

12
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp
View File

@@ -80,7 +80,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
if (!traj_generator_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
}
robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
traj_generator_->initialize(nh_traj_gen);
@@ -88,7 +88,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
catch (const std::exception& ex)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
}
std::string algorithm_nav_name = std::string("pnkx_local_planner::GoStraight");
@@ -104,14 +104,14 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
if (!nav_algorithm_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_nav_name.c_str());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
}
nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
}
catch (const std::exception& ex)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
}
std::string goal_checker_name;
@@ -126,7 +126,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
if (!goal_checker_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
}
robot::NodeHandle nh_goal_checker = robot::NodeHandle(parent_, goal_checker_name);
goal_checker_->initialize(nh_goal_checker);
@@ -134,7 +134,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
catch (const std::exception& ex)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s Goal checker , are you sure it is properly registered and that the containing library is built? Exception: %s", goal_checker_name.c_str(), ex.what());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
}
robot::log_info_at(__FILE__, __LINE__, "%s is initialized", name.c_str());

16
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp
View File

@@ -88,7 +88,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
if (!traj_generator_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
}
robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
traj_generator_->initialize(nh_traj_gen);
@@ -97,7 +97,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
catch (const std::exception &ex)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
}
std::string algorithm_nav_name;
@@ -113,14 +113,14 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
if (!nav_algorithm_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
}
nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
}
catch (const std::exception &ex)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
}
std::string algorithm_rotate_name;
@@ -135,7 +135,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
if (!rotate_algorithm_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
}
rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());
@@ -143,7 +143,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
catch (const std::exception &ex)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
}
std::string goal_checker_name;
@@ -159,7 +159,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
if (!goal_checker_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
}
goal_checker_->initialize(parent_);
robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
@@ -167,7 +167,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
catch (const std::exception &ex)
{
robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
}
this->initializeOthers();

12
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp
View File

@@ -64,7 +64,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
if (!traj_generator_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
}
robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
traj_generator_->initialize(nh_traj_gen);
@@ -72,7 +72,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
catch (const std::exception& ex)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
}
std::string algorithm_rotate_name = std::string("nav_algorithm::Rotate");
@@ -88,7 +88,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
if (!nav_algorithm_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
}
robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_rotate_name);
nav_algorithm_->initialize(nh_algorithm, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
@@ -96,7 +96,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
catch (const std::exception& ex)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
}
std::string goal_checker_name;
@@ -111,7 +111,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
if (!goal_checker_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
}
robot::NodeHandle nh_goal_checker = robot::NodeHandle(parent_, goal_checker_name);
goal_checker_->initialize(nh_goal_checker);
@@ -119,7 +119,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
catch (const std::exception& ex)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s Goal checker , are you sure it is properly registered and that the containing library is built? Exception: %s", goal_checker_name.c_str(), ex.what());
exit(1);
throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
}
robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str());