diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml index 39116cf..f606ea9 100755 --- a/config/costmap_common_params.yaml +++ b/config/costmap_common_params.yaml @@ -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 diff --git a/config/hybrid_local_planner_params.yaml b/config/hybrid_local_planner_params.yaml deleted file mode 100644 index 408042f..0000000 --- a/config/hybrid_local_planner_params.yaml +++ /dev/null @@ -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 \ No newline at end of file diff --git a/src/Navigations/Packages/move_base/src/convert_data.cpp b/src/Navigations/Packages/move_base/src/convert_data.cpp index 8e8f12d..c63f778 100644 --- a/src/Navigations/Packages/move_base/src/convert_data.cpp +++ b/src/Navigations/Packages/move_base/src/convert_data.cpp @@ -1,7 +1,6 @@ #include #include #include -#include 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 - // lock(*(costmap_->getCostmap()->getMutex())); - + // boost::unique_lock 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]) << " "; - } - file << "\n"; + grid_.data[i] = cost_translation_table_[ data[ i ]]; } - - file.close(); } - void move_base::ConvertData::updateCostmap(robot_nav_msgs::OccupancyGrid &grid, robot_map_msgs::OccupancyGridUpdate &update, bool &is_updated) { boost::unique_lock lock(*(costmap_->getCostmap()->getMutex()));