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

Merge branch '3.0' of https://git.pnkr.asia/HiepLM/pnkx_nav_core into 3.0

Browse Source
This commit is contained in:
HiepLM
2026-03-19 09:40:33 +07:00
parent f0d987da39 6d3af679a9
commit c36f3737ba
6 changed files with 21 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.gitmodules vendored
View File

@@ -28,3 +28,6 @@
[submodule "src/Libraries/xmlrpcpp"] [submodule "src/Libraries/xmlrpcpp"]
path = src/Libraries/xmlrpcpp path = src/Libraries/xmlrpcpp
url = https://git.pnkr.asia/DuongTD/xmlrpcpp.git url = https://git.pnkr.asia/DuongTD/xmlrpcpp.git
[submodule "src/Libraries/laser_filter"]
path = src/Libraries/laser_filter
url = https://git.pnkr.asia/DuongTD/laser_filter.git

4
CMakeLists.txt
View File

@@ -74,6 +74,10 @@ if (NOT TARGET robot_nav_2d_utils)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_nav_2d_utils) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_nav_2d_utils)
endif() endif()
if (NOT TARGET laser_filter)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/laser_filter)
endif()
if (NOT TARGET robot_nav_core) if (NOT TARGET robot_nav_core)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core)
endif() endif()

6
config/pnkx_local_planner_params.yaml
View File

@@ -41,8 +41,8 @@ PNKXRotateLocalPlanner:
LimitedAccelGenerator: LimitedAccelGenerator:
library_path: libmkt_plugins_standard_traj_generator library_path: libmkt_plugins_standard_traj_generator
max_vel_x: 0.2 max_vel_x: 1.0
min_vel_x: -0.2 min_vel_x: -1.0
max_vel_y: 0.0 # diff drive robot max_vel_y: 0.0 # diff drive robot
min_vel_y: 0.0 # diff drive robot min_vel_y: 0.0 # diff drive robot
@@ -50,7 +50,7 @@ LimitedAccelGenerator:
max_speed_xy: 2.0 # max_trans_vel: 0.8 # choose slightly less than the base's capability max_speed_xy: 2.0 # max_trans_vel: 0.8 # choose slightly less than the base's capability
min_speed_xy: 0.25 # min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity min_speed_xy: 0.25 # min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity
max_vel_theta: 0.7 # max_rot_vel: 1.0 # choose slightly less than the base's capability max_vel_theta: 0.4 # max_rot_vel: 1.0 # choose slightly less than the base's capability
min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity
acc_lim_x: 1.5 acc_lim_x: 1.5

1
src/Libraries/laser_filter Submodule

Submodule src/Libraries/laser_filter added at db25b6bb28

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

@@ -45,6 +45,7 @@ if (NOT BUILDING_WITH_CATKIN)
robot_nav_2d_utils robot_nav_2d_utils
robot_cpp robot_cpp
robot_move_base_msgs robot_move_base_msgs
laser_filter
) )
find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu) find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
else() else()

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

@@ -31,6 +31,7 @@
#include <robot_sensor_msgs/PointCloud.h> #include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h> #include <robot_sensor_msgs/PointCloud2.h>
#include <robot_nav_2d_utils/conversions.h> #include <robot_nav_2d_utils/conversions.h>
#include <laser_filter/laser_filter.h>
move_base::MoveBase::MoveBase() move_base::MoveBase::MoveBase()
: initialized_(false), : initialized_(false),
@@ -499,13 +500,17 @@ robot_nav_msgs::OccupancyGrid move_base::MoveBase::getStaticMap(const std::strin
void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan)
{ {
auto it = laser_scans_.find(laser_scan_name); auto it = laser_scans_.find(laser_scan_name);
laser_filter::LaserScanSOR sor;
sor.setMeanK(10); // xét 10 điểm láng giềng gần nhất
sor.setStddevMulThresh(1.0); // ngưỡng = mean + 1.0 * stddev
robot_sensor_msgs::LaserScan laser_scan_filter = sor.filter(laser_scan);
if (it == laser_scans_.end()) if (it == laser_scans_.end())
{ {
laser_scans_[laser_scan_name] = laser_scan; laser_scans_[laser_scan_name] = laser_scan_filter;
} }
else else
{ {
it->second = laser_scan; it->second = laser_scan_filter;
} }
// robot::log_info("stamp: %ld.%ld", laser_scan.header.stamp.sec, laser_scan.header.stamp.nsec); // robot::log_info("stamp: %ld.%ld", laser_scan.header.stamp.sec, laser_scan.header.stamp.nsec);
// robot::log_info("frame_id: %s", laser_scan.header.frame_id.c_str()); // robot::log_info("frame_id: %s", laser_scan.header.frame_id.c_str());
@@ -529,8 +534,8 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot
// } // }
// robot::log_error("intensities: %s", intensities_str.str().c_str()); // robot::log_error("intensities: %s", intensities_str.str().c_str());
updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
} }
robot_sensor_msgs::LaserScan move_base::MoveBase::getLaserScan(const std::string &laser_scan_name) robot_sensor_msgs::LaserScan move_base::MoveBase::getLaserScan(const std::string &laser_scan_name)