Add recovery and mission manager structure

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

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

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
+        footprint_clearing_enabled: true

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

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:

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

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:

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

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

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
+    

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

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;

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;

src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h

@@ -174,7 +174,8 @@ namespace robot
 		{
 		public:
 			using Ptr = std::shared_ptr<BaseNavigation>;
-
+			robot_nav_msgs::OccupancyGrid map_save_;
+    		std::string map_name_save_;
 			virtual ~BaseNavigation() {}
 
 			/**

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.

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<robot_nav_msgs::OccupancyGrid>(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name);
   updateLocalCostmap<robot_nav_msgs::OccupancyGrid>(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<T>(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