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

test finish

Browse Source
This commit is contained in:
HiepLM
2025-12-25 15:47:10 +07:00
parent 6c4d21826b
commit 2fb4827c00
11 changed files with 193 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,10 +5,6 @@
#include <nav_2d_utils/tf_help.h>
#include <nav_2d_utils/path_ops.h>
#include <nav_2d_utils/parameters.h>
// #include <g2o/core/base_binary_edge.h>
// #include <g2o/core/base_unary_edge.h>
// #include <g2o/core/base_multi_edge.h>
#include <angles/angles.h>
#include <tf3/convert.h>
@@ -18,6 +14,7 @@
#include <mkt_msgs/Trajectory2D.h>
#include <boost/dll/import.hpp>
#include <boost/dll/alias.hpp>
#include <robot/plugin_loader_helper.h>
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<score_algorithm::TrajectoryGenerator::Ptr()>(
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<score_algorithm::ScoreAlgorithm::Ptr()>(
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<score_algorithm::ScoreAlgorithm::Ptr()>(
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<score_algorithm::GoalChecker::Ptr()>(
path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
goal_checker_ = goal_checker_loader_();

2
src/Libraries/costmap_2d

Submodule src/Libraries/costmap_2d updated: 10521c1629...0344c31e5b

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

@@ -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<nav_core2::LocalPlanner::Ptr()>(
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)

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

@@ -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

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

@@ -17,6 +17,11 @@
#include <boost/dll/alias.hpp>
#include <robot/plugin_loader_helper.h>
#include <costmap_2d/static_layer.h>
#include <costmap_2d/voxel_layer.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/LaserScan.h>
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<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
layer != layered_costmap_->getPlugins()->end();
++layer)
{
boost::shared_ptr<costmap_2d::StaticLayer> static_layer = boost::dynamic_pointer_cast<costmap_2d::StaticLayer>(*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<nav_msgs::OccupancyGrid>(*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<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
layer != layered_costmap_->getPlugins()->end();
++layer)
{
boost::shared_ptr<costmap_2d::VoxelLayer> obstacle_layer = boost::dynamic_pointer_cast<costmap_2d::VoxelLayer>(*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<sensor_msgs::LaserScan>(*laser_scan, "laser");
}
}
catch (const std::exception &ex)
{

77
src/Navigations/Packages/move_base/src/move_base_main.cpp
View File

@@ -33,15 +33,65 @@
#include <nav_2d_utils/parameters.h>
#include <boost/dll/import.hpp>
#include <robot/console.h>
#include <tf3/buffer_core.h>
#include <tf3/transform_datatypes.h>
int main(int argc, char** argv)
{
try {
std::shared_ptr<tf3::BufferCore> tf = std::make_shared<tf3::BufferCore>();
// 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<move_base_core::BaseNavigation::Ptr()>(
"/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;
}