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

fix

Browse Source
This commit is contained in:
HiepLM
2026-03-11 15:11:59 +07:00
parent 9e7d98934d
commit ae2f647fc9
3 changed files with 23 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
config/costmap_common_params.yaml
View File

@@ -1,5 +1,4 @@
# position_planner_name: PNKXLocalPlanner
position_planner_name: HybridLocalPlanner
position_planner_name: PNKXLocalPlanner
docking_planner_name: PNKXDockingLocalPlanner
go_straight_planner_name: PNKXGoStraightLocalPlanner
rotate_planner_name: PNKXRotateLocalPlanner
@@ -27,29 +26,29 @@ virtual_walls_map:
lethal_cost_threshold: 100
obstacles:
observation_sources: b_scan_marking b_scan_clearing #f_scan_marking f_scan_clearing
# f_scan_marking:
# topic: /f_scan
# data_type: LaserScan
# clearing: false
# marking: true
# inf_is_valid: false
# min_obstacle_height: 0.0
# max_obstacle_height: 0.25
# f_scan_clearing:
# topic: /f_scan
# data_type: LaserScan
# clearing: true
# marking: false
# inf_is_valid: false
# min_obstacle_height: 0.0
# max_obstacle_height: 0.25
observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
f_scan_marking:
topic: /f_scan
data_type: LaserScan
clearing: false
marking: true
inf_is_valid: true
min_obstacle_height: 0.0
max_obstacle_height: 0.25
f_scan_clearing:
topic: /f_scan
data_type: LaserScan
clearing: true
marking: false
inf_is_valid: true
min_obstacle_height: 0.0
max_obstacle_height: 0.25
b_scan_marking:
topic: /b_scan
data_type: LaserScan
clearing: false
marking: true
inf_is_valid: false
inf_is_valid: true
min_obstacle_height: 0.0
max_obstacle_height: 0.25
b_scan_clearing:
@@ -57,7 +56,7 @@ obstacles:
data_type: LaserScan
clearing: true
marking: false
inf_is_valid: false
inf_is_valid: true
min_obstacle_height: 0.0
max_obstacle_height: 0.25

54
config/hybrid_local_planner_params.yaml
View File

@@ -1,54 +0,0 @@
LocalPlannerAdapter:
library_path: liblocal_planner_adapter
yaw_goal_tolerance: 0.017
xy_goal_tolerance: 0.03
min_approach_linear_velocity: 0.06
HybridLocalPlanner:
# base_local_planner: "hybrid_local_planner/HybridLocalPlanner"
# HybridLocalPlanner:
library_path: libhybrid_local_planner
odom_topic: odom
# Trajectory
max_global_plan_lookahead_dist: 4.0
global_plan_viapoint_sep: 0.5
global_plan_prune_distance: 0.0
global_plan_goal_sep: 0.05
# Robot
max_vel_x: 1.0
max_vel_x_backwards: -0.2
max_vel_theta: 0.5
acc_lim_x: 1.0
acc_lim_theta: 2.0
turn_around_priority: True
stop_dist: 0.5
dec_dist: 1.0
# GoalTolerance
xy_goal_tolerance: 0.05
yaw_goal_tolerance: 0.05
# Optimization
# PP Parameters
w_vel: 0.9
w_omega: 1.12
# DWA Parameters
robot_max_v: 0.4
robot_max_w: 0.4
enable_backward_motion: False
w_targetheading: 1.7
w_velocity: 0.2
w_clearance: 0.2
w_pathDistance: 0.05
time_horizon: 3.0
velocity_resolution: 0.015
segment_transition_threshold: 0.01 # Ngưỡng khoảng cách chuyển segment
calibration_factor: 1.5 # Hệ số hiệu chuẩn
use_obstacle_avoidance: true # Bật tắt tránh vật cản

41
src/Navigations/Packages/move_base/src/convert_data.cpp
View File

@@ -1,7 +1,6 @@
#include <move_base/convert_data.h>
#include <robot/robot.h>
#include <robot_costmap_2d/cost_values.h>
#include <fstream>
char* move_base::ConvertData::cost_translation_table_ = NULL;
@@ -42,19 +41,7 @@ move_base::ConvertData::~ConvertData()
// prepare grid_ message for publication.
void move_base::ConvertData::prepareGrid()
{
printf("Preparing costmap data for publication\n");
std::ofstream file("/home/robotics/sonvh/pnkx_nav_core/obstacle/hybrid_local_planner/cfg/costmap_data.txt");
if (!file.is_open())
{
std::cout << "Cannot open file!" << std::endl;
return;
}
// boost::unique_lock<costmap_2d::Costmap2D::mutex_t>
// lock(*(costmap_->getCostmap()->getMutex()));
// boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
double resolution = costmap_->getCostmap()->getResolution();
grid_.header.frame_id = global_frame_;
@@ -66,44 +53,22 @@ void move_base::ConvertData::prepareGrid()
double wx, wy;
costmap_->getCostmap()->mapToWorld(0, 0, wx, wy);
grid_.info.origin.position.x = wx - resolution / 2;
grid_.info.origin.position.y = wy - resolution / 2;
grid_.info.origin.position.z = 0.0;
grid_.info.origin.orientation.w = 1.0;
saved_origin_x_ = costmap_->getCostmap()->getOriginX();
saved_origin_y_ = costmap_->getCostmap()->getOriginY();
grid_.data.resize(grid_.info.width * grid_.info.height);
file << "frame: " << global_frame_ << std::endl;
file << "time: " << robot::Time::now().toSec() << std::endl;
file << "resolution: " << resolution << std::endl;
file << "width: " << grid_.info.width << std::endl;
file << "height: " << grid_.info.height << std::endl;
file << "origin_x: " << grid_.info.origin.position.x << std::endl;
file << "origin_y: " << grid_.info.origin.position.y << std::endl;
unsigned char* data = costmap_->getCostmap()->getCharMap();
for (unsigned int y = 0; y < grid_.info.height; y++)
for (unsigned int i = 0; i < grid_.data.size(); i++)
{
for (unsigned int x = 0; x < grid_.info.width; x++)
{
unsigned int i = y * grid_.info.width + x;
grid_.data[i] = cost_translation_table_[data[i]];
file << int(grid_.data[i]) << " ";
grid_.data[i] = cost_translation_table_[ data[ i ]];
}
file << "\n";
}
file.close();
}
void move_base::ConvertData::updateCostmap(robot_nav_msgs::OccupancyGrid &grid, robot_map_msgs::OccupancyGridUpdate &update, bool &is_updated)
{
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getCostmap()->getMutex()));