diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml index 997b55c..efb6335 100755 --- a/config/costmap_common_params.yaml +++ b/config/costmap_common_params.yaml @@ -11,7 +11,7 @@ navigation_map: map_file: maze virtual_walls_map: - map_topic: /virtual_walls/map + map_topic: /map namespace: /virtual_walls map_pkg: managerments map_file: maze @@ -19,23 +19,23 @@ virtual_walls_map: lethal_cost_threshold: 100 obstacles: - 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 + observation_sources: b_scan_marking b_scan_clearing pc_marking pc_clearing + # f_scan_marking: f_scan_marking f_scan_clearing + # 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 @@ -53,4 +53,28 @@ obstacles: min_obstacle_height: 0.0 max_obstacle_height: 0.25 + pc_marking: + topic: /camera/depth/points_proc + data_type: PointCloud2 + clearing: false + marking: true + inf_is_valid: false + observation_persistence: 0.0 + expected_update_rate: 0.5 + obstacle_range: 2.5 + raytrace_range: 3.0 + min_obstacle_height: 0.1 + max_obstacle_height: 1.0 + pc_clearing: + topic: /camera/depth/points_clear + data_type: PointCloud2 + clearing: true + marking: false + inf_is_valid: false + observation_persistence: 0.0 + expected_update_rate: 0.5 + obstacle_range: 2.5 + raytrace_range: 3.0 + min_obstacle_height: -0.5 + max_obstacle_height: 5.0 diff --git a/config/costmap_global_params.yaml b/config/costmap_global_params.yaml index 811ab13..d3d9b00 100755 --- a/config/costmap_global_params.yaml +++ b/config/costmap_global_params.yaml @@ -4,7 +4,7 @@ global_costmap: global_frame: map update_frequency: 1.0 publish_frequency: 1.0 - raytrace_range: 2.0 + raytrace_range: 3.0 resolution: 0.05 z_resolution: 0.2 rolling_window: false diff --git a/config/costmap_global_params_plugins_no_virtual_walls.yaml b/config/costmap_global_params_plugins_no_virtual_walls.yaml index ce17e0f..30a3e96 100755 --- a/config/costmap_global_params_plugins_no_virtual_walls.yaml +++ b/config/costmap_global_params_plugins_no_virtual_walls.yaml @@ -7,4 +7,4 @@ global_costmap: - {name: inflation, type: "InflationLayer" } obstacles: enabled: false - footprint_clearing_enabled: false \ No newline at end of file + footprint_clearing_enabled: true \ No newline at end of file diff --git a/config/costmap_local_params.yaml b/config/costmap_local_params.yaml index ecd9e9d..a4b6871 100755 --- a/config/costmap_local_params.yaml +++ b/config/costmap_local_params.yaml @@ -5,7 +5,7 @@ local_costmap: update_frequency: 6.0 publish_frequency: 6.0 rolling_window: true - raytrace_range: 2.0 + raytrace_range: 3.0 resolution: 0.05 z_resolution: 0.15 z_voxels: 8 diff --git a/config/costmap_local_params_plugins_no_virtual_walls.yaml b/config/costmap_local_params_plugins_no_virtual_walls.yaml index 84936bb..a9309cb 100755 --- a/config/costmap_local_params_plugins_no_virtual_walls.yaml +++ b/config/costmap_local_params_plugins_no_virtual_walls.yaml @@ -1,6 +1,7 @@ local_costmap: frame_id: odom plugins: + # - {name: virtual_walls_map, type: "StaticLayer" } - {name: obstacles, type: "VoxelLayer" } - {name: inflation, type: "InflationLayer" } obstacles: diff --git a/config/hybrid_local_planner_params.yaml b/config/hybrid_local_planner_params.yaml new file mode 100644 index 0000000..213875a --- /dev/null +++ b/config/hybrid_local_planner_params.yaml @@ -0,0 +1,64 @@ +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 + # Robot + robot_max_v_ac: 0.4 + robot_max_w_ac: 0.6 + robot_max_v_pt: 1.0 + robot_max_w_pt: 0.6 + robot_min_v: -0.5 + robot_min_w: 0.05 + robot_max_v_backwards_pt: -0.25 + acc_lim_x: 1.0 + acc_lim_theta: 2.0 + min_turn_radius: 0.0 + turn_around_priority: true + stop_dist: 0.5 + dec_dist: 1.0 + max_acceleration: 0.1 + max_deceleration: 0.5 + max_d_yawrate: 3.2 + max_in_place_yawrate: 0.6 + min_in_place_yawrate: 0.3 + robot_radius: 0.1 + footprint_padding: 0.08 + use_footprint: true + + # GoalTolerance + xy_goal_tolerance: 0.02 + yaw_goal_tolerance: 0.02 + + # Trajectory + max_global_plan_lookahead_dist: 3.0 + global_plan_viapoint_sep: 0.5 + global_plan_goal_sep: 0.05 + global_plan_prune_distance: 0.0 + predict_time: 3.0 + sim_period: 0.1 + sim_time_samples: 10 + sim_direction: M_PI / 2.0 + + # Optimization + w_vel: 0.8 + w_omega: 2.5 + obs_cost_gain: 1.3 + path_cost_gain: 2.0 + to_goal_cost_gain: 0.8 + speed_cost_gain: 0.8 + + # Obstacles + obs_range: 2.5 + + #GeneralSetting + segment_transition_threshold: 0.01 + calibration_factor: 1.5 + use_obstacle_avoidance: true + slow_velocity_th: 0.1 + turn_direction_th: 0.1 \ No newline at end of file diff --git a/config/move_base_common_params.yaml b/config/move_base_common_params.yaml index 1fa1831..b1c056b 100755 --- a/config/move_base_common_params.yaml +++ b/config/move_base_common_params.yaml @@ -1,18 +1,43 @@ -position_planner_name: PNKXLocalPlanner +# position_planner_name: PNKXLocalPlanner +# docking_planner_name: PNKXDockingLocalPlanner +# position_planner_name: StanleyLocalPlanner +# docking_planner_name: StanleyDockingLocalPlanner +position_planner_name: MPPILocalPlanner #HybridLocalPlanner MPPILocalPlanner docking_planner_name: PNKXDockingLocalPlanner go_straight_planner_name: PNKXGoStraightLocalPlanner rotate_planner_name: PNKXRotateLocalPlanner base_local_planner: LocalPlannerAdapter base_global_planner: CustomPlanner -PNKXLocalPlanner: +# PNKXLocalPlanner: +# base_local_planner: LocalPlannerAdapter +# base_global_planner: SBPLLatticePlanner + +# PNKXDockingLocalPlanner: +# base_local_planner: LocalPlannerAdapter +# base_global_planner: CustomPlanner + +# StanleyLocalPlanner: +# base_local_planner: LocalPlannerAdapter +# base_global_planner: CustomPlanner + +# StanleyDockingLocalPlanner: +# base_local_planner: LocalPlannerAdapter +# base_global_planner: CustomPlanner + +# HybridLocalPlanner: +# base_local_planner: LocalPlannerAdapter +# base_global_planner: CustomPlanner + +MPPILocalPlanner: base_local_planner: LocalPlannerAdapter - base_global_planner: CustomPlanner + base_global_planner: SBPLLatticePlanner PNKXDockingLocalPlanner: base_local_planner: LocalPlannerAdapter base_global_planner: TwoPointsPlanner + PNKXGoStraightLocalPlanner: base_local_planner: LocalPlannerAdapter base_global_planner: TwoPointsPlanner @@ -30,10 +55,10 @@ max_planning_retries: 0 # ... or after 10 attempts (whichever happens first) oscillation_timeout: -1 # abort controller and trigger recovery behaviors after 30.0 s oscillation_distance: 0.5 ### recovery behaviors -recovery_behavior_enabled: false +recovery_behavior_enabled: true recovery_behaviors: [ {name: aggressive_reset, type: ClearCostmapRecovery}, - {name: conservative_reset, type: ClearCostmapRecovery}, + {name: conservative_reset, type: ClearCostmapRecovery} ] conservative_reset: diff --git a/config/mppi_local_planner_params.yaml b/config/mppi_local_planner_params.yaml new file mode 100644 index 0000000..28f8e15 --- /dev/null +++ b/config/mppi_local_planner_params.yaml @@ -0,0 +1,126 @@ +LocalPlannerAdapter: + library_path: liblocal_planner_adapter + yaw_goal_tolerance: 0.017 + xy_goal_tolerance: 0.03 + min_approach_linear_velocity: 0.06 + +MPPILocalPlanner: + library_path: libmppi_local_planner + + # Robot limits [m/s, rad/s, m/s^2, rad/s^2] + max_velocity: 0.5 + min_velocity: 0.0 + max_y_velocity: 0.0 + min_command_velocity: 0.05 + max_angular_velocity: 1.9 + min_angular_velocity: 0.05 + max_acceleration: 0.5 + max_angular_acceleration: 1.0 + max_vel_trans: 0.5 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + motion_model: 0 + min_turning_radius: 0.2 + + # Goal tolerance [m, rad] + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.1 + stopped_xy_velocity: 0.03 + stopped_theta_velocity: 0.03 + + # MPPI horizon and global-plan handling + model_dt: 0.05 + time_steps: 56 + batch_size: 1000 + iteration_count: 1 + temperature: 0.3 + gamma: 0.1 + retry_attempt_limit: 10 + regenerate_noises: false + open_loop: false + controller_frequency: 30.0 + prune_distance: 1.5 + max_robot_pose_search_dist: 0.0 + max_global_plan_lookahead_dist: 3.0 + global_plan_prune_distance: 0.2 + sim_period: 0.1 + + # Costmap safety + use_costmap_collision_check: true + allow_unknown: true + lethal_cost: 253 + inflation_score_distance: 0.3 + consider_footprint: true + + # Critic defaults ported from the legacy MPPI dynamic_reconfigure files. + constraint_enabled: true + constraint_power: 1.0 + constraint_weight: 4.0 + + cost_enabled: true + cost_power: 1.0 + cost_weight: 3.81 + cost_critical_cost: 300.0 + cost_near_collision_cost: 253 + cost_collision_cost: 1000000.0 + cost_near_goal_distance: 1.0 + cost_trajectory_point_step: 2 + + goal_enabled: true + goal_power: 1.0 + goal_weight: 5.0 + goal_threshold_to_consider: 1.4 + + goal_angle_enabled: true + goal_angle_power: 1.0 + goal_angle_weight: 3.0 + goal_angle_threshold_to_consider: 0.5 + + obstacles_enabled: false + obstacles_power: 1.0 + obstacles_weight: 0.0 + obstacles_repulsion_weight: 1.5 + obstacles_collision_cost: 10000.0 + obstacles_collision_margin_distance: 0.1 + obstacles_near_goal_distance: 0.5 + + path_align_enabled: true + path_align_power: 1.0 + path_align_weight: 8.0 + path_align_max_path_occupancy_ratio: 0.07 + path_align_threshold_to_consider: 0.5 + path_align_offset_from_furthest: 20 + path_align_trajectory_point_step: 4 + path_align_use_path_orientations: false + + path_angle_enabled: true + path_angle_power: 1.0 + path_angle_weight: 2.0 + path_angle_max_angle_to_furthest: 1.2 + path_angle_threshold_to_consider: 0.5 + path_angle_offset_from_furthest: 4 + path_angle_mode: 0 + + path_follow_enabled: true + path_follow_power: 1.0 + path_follow_weight: 5.0 + path_follow_threshold_to_consider: 1.4 + path_follow_offset_from_furthest: 6 + + prefer_forward_enabled: true + prefer_forward_power: 1.0 + prefer_forward_weight: 1.0 + prefer_forward_threshold_to_consider: 0.5 + + twirling_enabled: false + twirling_power: 1.0 + twirling_weight: 0.0 + twirling_threshold_to_consider: 0.5 + + velocity_deadband_enabled: false + velocity_deadband_power: 1.0 + velocity_deadband_weight: 0.0 + velocity_deadband_deadband_velocities: [0.0, 0.0, 0.0] + + publish_debug: false diff --git a/config/sbpl_global_params.yaml b/config/sbpl_global_params.yaml new file mode 100644 index 0000000..c619680 --- /dev/null +++ b/config/sbpl_global_params.yaml @@ -0,0 +1,11 @@ +SBPLLatticePlanner: + library_path: libsbpl_lattice_planner + environment_type: XYThetaLattice + planner_type: ARAPlanner + allocated_time: 10.0 + initial_epsilon: 1.0 + force_scratch_limit: 10000 + forward_search: true + nominalvel_mpersecs: 0.3 + timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s + primitive_filename: /home/duongtd/T800_ws/src/AMR_T800/Test/sbpl_lattice_planner/matlab/mprim/unicycle_highcost_5cm.mprim \ No newline at end of file diff --git a/config/stanley_local_planner_params.yaml b/config/stanley_local_planner_params.yaml new file mode 100644 index 0000000..494379c --- /dev/null +++ b/config/stanley_local_planner_params.yaml @@ -0,0 +1,14 @@ +LocalPlannerAdapter: + library_path: liblocal_planner_adapter + yaw_goal_tolerance: 0.017 + xy_goal_tolerance: 0.03 + min_approach_linear_velocity: 0.06 + +StanleyLocalPlanner: + # Algorithm + library_path: libstanley_local_planner + +StanleyDockingLocalPlanner: + # Algorithm + library_path: libstanley_local_planner + \ No newline at end of file diff --git a/src/APIs/c_api/include/nav_c_api_io_structure.md b/src/APIs/c_api/include/nav_c_api_io_structure.md new file mode 100644 index 0000000..e69de29 diff --git a/src/Algorithms/Packages/global_planners/custom_planner b/src/Algorithms/Packages/global_planners/custom_planner index 49ea9fe..7c84705 160000 --- a/src/Algorithms/Packages/global_planners/custom_planner +++ b/src/Algorithms/Packages/global_planners/custom_planner @@ -1 +1 @@ -Subproject commit 49ea9fe7f5e2a8a66d38ba269100339509b609e9 +Subproject commit 7c84705ce737736d53829916eb36c9860c10c418 diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp index d3064b3..48f15fc 100755 --- a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp +++ b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp @@ -45,6 +45,8 @@ namespace two_points_planner name_ = name; costmap_robot_ = costmap_robot; + + if(!costmap_robot_ || !costmap_robot_->getCostmap()) { @@ -55,6 +57,8 @@ namespace two_points_planner current_env_height_ = costmap_robot_->getCostmap()->getSizeInCellsY(); footprint_ = costmap_robot_->getRobotFootprint(); + robot::log_info("size x: %d, size y: %d, resolution: %f", costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY(), costmap_robot_->getCostmap()->getResolution()); + robot::log_info("TwoPointsPlanner Initialized successfully"); initialized_ = true; return true; @@ -119,6 +123,18 @@ namespace two_points_planner robot::log_error("[%s:%d]\n TwoPointsPlanner: Global planner is not initialized", __FILE__, __LINE__); return false; } + + robot::Time start_time = robot::Time::now(); + robot::Rate rate(1.0); + while(costmap_robot_->getCostmap()->getSizeInCellsX() == 0 || costmap_robot_->getCostmap()->getSizeInCellsY() == 0){ + robot::log_warning("Waiting for costmap to be initialized..."); + rate.sleep(); + if((robot::Time::now() - start_time).toSec() > 5.0){ + robot::log_error("Costmap not initialized after 10 seconds, exiting..."); + exit(1); + } + } + robot::log_warning("abc testttttt!, SizeInCellsX = %d, SizeInCellsY = %d", costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY()); robot_nav_2d_msgs::Pose2DStamped start_2d = robot_nav_2d_utils::poseStampedToPose2D(start); robot_nav_2d_msgs::Pose2DStamped goal_2d = robot_nav_2d_utils::poseStampedToPose2D(goal); diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp index d1a141a..f09d811 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp @@ -366,6 +366,7 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::c const robot_nav_2d_msgs::Twist2D &velocity) { // boost::recursive_mutex::scoped_lock l(configuration_mutex_); + robot::log_error("DEBUG 300"); robot_nav_2d_msgs::Twist2DStamped cmd_vel; try { @@ -424,9 +425,9 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_ robot::log_warning_at(__FILE__, __LINE__, "Cannot check if the goal is reached without the goal being set!"); return false; } - + robot::log_error("DEBUG 400.1"); bool dock_ok = dockingHanlde(pose, velocity); - + robot::log_error("DEBUG 400.2"); // Update time stamp of goal pose // goal_pose_.header.stamp = pose.header.stamp; robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose); @@ -503,6 +504,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_ { if (dkpl_.front()->geLocalGoal(local_goal)) { + robot::log_error("DEBUG 100"); dkpl_.front()->is_detected_ = true; start_docking_ = true; robot_nav_msgs::Path path; @@ -524,6 +526,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_ { if (dkpl_.front()->geLocalGoal(local_goal)) { + robot::log_error("DEBUG 200"); dkpl_.front()->is_detected_ = true; start_docking_ = true; robot_nav_2d_msgs::Path2D path; diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp index aa614ab..c595656 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp @@ -208,12 +208,14 @@ void pnkx_local_planner::PNKXLocalPlanner::reset() void pnkx_local_planner::PNKXLocalPlanner::setGoalPose(const robot_nav_2d_msgs::Pose2DStamped &goal_pose) { // boost::recursive_mutex::scoped_lock l(configuration_mutex_); + robot::log_error("[PNKXLocalPlanner] Receive new goal(%f, %f)!", goal_pose.pose.x, goal_pose.pose.y); reset(); goal_pose_ = goal_pose; } void pnkx_local_planner::PNKXLocalPlanner::setPlan(const robot_nav_2d_msgs::Path2D &path) { + robot::log_error("[PNKXLocalPlanner] size path: %d", (int)path.poses.size()); // boost::recursive_mutex::scoped_lock l(configuration_mutex_); costmap_robot_->resetLayers(); global_plan_ = path; diff --git a/src/Libraries/costmap_2d.zip b/src/Libraries/costmap_2d.zip new file mode 100644 index 0000000..15ba03d Binary files /dev/null and b/src/Libraries/costmap_2d.zip differ diff --git a/src/Libraries/data_convert b/src/Libraries/data_convert index 004dd7f..1cf62c0 160000 --- a/src/Libraries/data_convert +++ b/src/Libraries/data_convert @@ -1 +1 @@ -Subproject commit 004dd7f0ffafe6b3d3a1c86e40bc7041b561756c +Subproject commit 1cf62c085687dc167e28cded970371b329a83479 diff --git a/src/Libraries/robot_time b/src/Libraries/robot_time index 75075a3..0c007fd 160000 --- a/src/Libraries/robot_time +++ b/src/Libraries/robot_time @@ -1 +1 @@ -Subproject commit 75075a34983c358f56f3b91e64754b53850a8b9c +Subproject commit 0c007fdab365be38bbf426657b741713114de34e diff --git a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h index 264f603..aef4d41 100755 --- a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h +++ b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h @@ -174,7 +174,8 @@ namespace robot { public: using Ptr = std::shared_ptr; - + robot_nav_msgs::OccupancyGrid map_save_; + std::string map_name_save_; virtual ~BaseNavigation() {} /** diff --git a/src/Navigations/Packages/move_base/include/move_base/move_base.h b/src/Navigations/Packages/move_base/include/move_base/move_base.h index 52c46fa..5628a8d 100644 --- a/src/Navigations/Packages/move_base/include/move_base/move_base.h +++ b/src/Navigations/Packages/move_base/include/move_base/move_base.h @@ -125,6 +125,7 @@ namespace move_base * @param laser_scan The laser scan to add. */ virtual void addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) override; + /** * @brief Add a point cloud to the navigation system. diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 11e92f2..08d49ab 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -266,6 +266,26 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) robot::log_error("[%s:%d]\n EXCEPTION: %s", __FILE__, __LINE__, ex.what()); throw std::runtime_error("Failed to create the planner_costmap_robot_"); } + + try + { + controller_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_); + if(controller_costmap_robot_ == nullptr) + { + robot::log_error("[%s:%d]\n ERROR: controller_costmap_robot_ is nullptr", __FILE__, __LINE__); + throw std::runtime_error("Failed to create the controller_costmap_robot_"); + } + controller_costmap_robot_->pause(); + robot_costmap_2d::LayeredCostmap *layered_costmap_ = controller_costmap_robot_->getLayeredCostmap(); + } + catch (const std::exception &ex) + { + robot::log_error("[%s:%d]\n EXCEPTION: %s", __FILE__, __LINE__, ex.what()); + throw std::runtime_error("Failed to create the controller_costmap_robot_"); + } + + addStaticMap(map_name_save_, map_save_); + // initialize the global planner try { @@ -294,22 +314,6 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) throw std::runtime_error("Failed to create the " + global_planner + " planner"); } - try - { - controller_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_); - if(controller_costmap_robot_ == nullptr) - { - robot::log_error("[%s:%d]\n ERROR: controller_costmap_robot_ is nullptr", __FILE__, __LINE__); - throw std::runtime_error("Failed to create the controller_costmap_robot_"); - } - controller_costmap_robot_->pause(); - robot_costmap_2d::LayeredCostmap *layered_costmap_ = controller_costmap_robot_->getLayeredCostmap(); - } - catch (const std::exception &ex) - { - robot::log_error("[%s:%d]\n EXCEPTION: %s", __FILE__, __LINE__, ex.what()); - throw std::runtime_error("Failed to create the controller_costmap_robot_"); - } // create a local planner try { @@ -487,7 +491,7 @@ void move_base::MoveBase::addStaticMap(const std::string &map_name, robot_nav_ms // for(size_t i = 0; i < map.data.size(); i++) { // robot::log_info("map data[%zu]: %d", i, map.data[i]); // } - // robot::log_info("--------------------------------"); + robot::log_info("--------------------------------"); updateGlobalCostmap(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name); updateLocalCostmap(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name); } @@ -729,8 +733,15 @@ void move_base::MoveBase::updateGlobalCostmap(const T& value, robot_costmap_2d:: { // Kiểm tra layer có đúng type và name không // Nếu layer type khớp HOẶC name khớp thì update + // if(layer->getType() == robot_costmap_2d::LayerType::STATIC_LAYER) + // robot::log_warning ("[%s:%d] Update GlobalCostmap: layer type: %d, name: %s, layer->getType(): %d, layer->getName(): %s", __FILE__, __LINE__, layer_type, name.c_str(), layer->getType(), layer->getName().c_str()); if (layer->getType() == layer_type || layer->getName() == name) { + if(layer->getType() == robot_costmap_2d::LayerType::STATIC_LAYER) + { + robot::log_warning("[%s:%d] Update GlobalCostmap: layer type is STATIC_LAYER, name: %s", __FILE__, __LINE__, name.c_str()); + } + // Update costmap với data layer->dataCallBack(value, name); } @@ -2864,7 +2875,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) { if (tc_->isGoalReached()) { - robot::log_debug("Goal reached!"); + robot::log_debug("[MoveBase] Goal reached!"); resetState(); // swapPlanner(default_config_.base_global_planner); // disable the planner thread