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)
endif()
if (NOT TARGET laser_filter)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/laser_filter)
endif()
if (NOT TARGET robot_nav_core)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core)
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_cpp
robot_move_base_msgs
laser_filter
)
find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
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/PointCloud2.h>
#include <robot_nav_2d_utils/conversions.h>
#include <laser_filter/laser_filter.h>
move_base::MoveBase::MoveBase()
: 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)
{
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())
{
laser_scans_[laser_scan_name] = laser_scan;
laser_scans_[laser_scan_name] = laser_scan_filter;
}
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("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());
updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, 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);
updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan_filter, 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)