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:
QUYVN
2026-03-17 10:02:02 +00:00
parent 3f1f762f9b
commit 1c12239478
3 changed files with 14 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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()

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)